001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package org.wpilib.math.autodiff;
006
007import org.ejml.simple.SimpleMatrix;
008
009/**
010 * This class calculates the Jacobian of a vector of variables with respect to a vector of
011 * variables.
012 *
013 * <p>The Jacobian is only recomputed if the variable expression is quadratic or higher order.
014 */
015public class Jacobian implements AutoCloseable {
016  private long m_handle;
017  private int m_rows;
018  private int m_cols;
019
020  /**
021   * Constructs a Jacobian object.
022   *
023   * @param variable Variable of which to compute the Jacobian.
024   * @param wrt Variable with respect to which to compute the Jacobian.
025   */
026  public Jacobian(Variable variable, Variable wrt) {
027    this(new VariableMatrix(variable), new VariableMatrix(wrt));
028  }
029
030  /**
031   * Constructs a Jacobian object.
032   *
033   * @param variable Variable of which to compute the Jacobian.
034   * @param wrt Vector of variables with respect to which to compute the Jacobian.
035   */
036  public Jacobian(Variable variable, VariableMatrix wrt) {
037    this(new VariableMatrix(variable), wrt);
038  }
039
040  /**
041   * Constructs a Jacobian object.
042   *
043   * @param variable Variable of which to compute the Jacobian.
044   * @param wrt Vector of variables with respect to which to compute the Jacobian.
045   */
046  public Jacobian(Variable variable, VariableBlock wrt) {
047    this(new VariableMatrix(variable), new VariableMatrix(wrt));
048  }
049
050  /**
051   * Constructs a Jacobian object.
052   *
053   * @param variables Vector of variables of which to compute the Jacobian.
054   * @param wrt Vector of variables with respect to which to compute the Jacobian.
055   */
056  public Jacobian(VariableMatrix variables, VariableMatrix wrt) {
057    assert variables.cols() == 1;
058    assert wrt.cols() == 1;
059
060    m_handle = JacobianJNI.create(variables.getHandles(), wrt.getHandles());
061    m_rows = variables.rows();
062    m_cols = wrt.rows();
063  }
064
065  /**
066   * Constructs a Jacobian object.
067   *
068   * @param variables Vector of variables of which to compute the Jacobian.
069   * @param wrt Vector of variables with respect to which to compute the Jacobian.
070   */
071  public Jacobian(VariableMatrix variables, VariableBlock wrt) {
072    this(variables, new VariableMatrix(wrt));
073  }
074
075  @Override
076  public void close() {
077    if (m_handle != 0) {
078      JacobianJNI.destroy(m_handle);
079      m_handle = 0;
080    }
081  }
082
083  /**
084   * Returns the Jacobian as a VariableMatrix.
085   *
086   * <p>This is useful when constructing optimization problems with derivatives in them.
087   *
088   * @return The Jacobian as a VariableMatrix.
089   */
090  public VariableMatrix get() {
091    return new VariableMatrix(m_rows, m_cols, JacobianJNI.get(m_handle));
092  }
093
094  /**
095   * Evaluates the Jacobian at wrt's value.
096   *
097   * @return The Jacobian at wrt's value.
098   */
099  public SimpleMatrix value() {
100    return JacobianJNI.value(m_handle).toSimpleMatrix(m_rows, m_cols);
101  }
102}