Drake
Drake C++ Documentation
Classes | Namespaces
linear_solve.h File Reference
#include <optional>
#include "drake/common/symbolic/expression.h"
#include "drake/math/autodiff.h"
#include "drake/math/autodiff_gradient.h"
Include dependency graph for linear_solve.h:
This graph shows which files directly or indirectly include this file:

Classes

class  LinearSolver< LinearSolverType, DerivedA >
 Solves a linear system of equations A*x=b. More...
 

Namespaces

 drake
 
 drake::math
 

Functions

solve linear system of equations with a given solver.

Solve linear system of equations A * x = b. Where A is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression, and b is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression. Notice that if either A or b contains symbolic::Expression, then the other has to contain symbolic::Expression. This 3-argument version allows the user to re-use linear_solver when b changes or the gradient of A changes. When either A or b contains AutoDiffScalar, we use implicit function theorem to find the gradient in x as ∂x/∂zᵢ = A⁻¹(∂b/∂zᵢ - ∂A/∂zᵢ * x) where z is the variable we take gradient with.

Note
When both A and b are Eigen matrix of double, this function is almost as fast as calling linear_solver.solve(b) directly. When either A or b contains AutoDiffScalar, this function is a lot faster than first instantiating the linear solver of AutoDiffScalar, and then solving the equation with this autodiffable linear solver.
Template Parameters
LinearSolverThe type of linear solver, for example Eigen::LLT<Eigen::Matrix2d>
DerivedAAn Eigen Matrix.
DerivedBAn Eigen Vector.
Parameters
linear_solverThe linear solver constructed with the double-version of A.
AThe matrix A.
bThe vector b.

Here is an example code.

Eigen::Matrix<AutoDiffd<3>, 2, 2> A_ad;
// Set the value and gradient in A_ad with arbitrary values;
Eigen::Matrix2d A_val;
A_val << 1, 2, 3, 4;
// Gradient of A.col(0).
Eigen::Matrix<double, 2, 3> A0_gradient;
A0_gradient << 1, 2, 3, 4, 5, 6;
A_ad.col(0) = InitializeAutoDiff(A_val.col(0), A0_gradient);
// Gradient of A.col(1)
Eigen::Matrix<double, 2, 3> A1_gradient;
A1_gradient << 7, 8, 9, 10, 11, 12;
A_ad.col(1) = InitializeAutoDiff(A_val.col(1), A1_gradient);
// Set the value and gradient of b to arbitrary value.
const Eigen::Vector2d b_val(2, 3);
Eigen::Matrix<double, 2, 3> b_gradient;
b_gradient << 1, 3, 5, 7, 9, 11;
const auto b_ad = InitializeAutoDiff(b_val, b_gradient);
// Solve the linear system A_val * x_val = b_val.
Eigen::PartialPivLU<Eigen::Matrix2d> linear_solver(A_val);
const auto x_val = SolveLinearSystem(linear_solver, A_val, b_val);
// Solve the linear system A*x=b, together with the gradient.
// x_ad contains both the value of the solution A*x=b, together with its
// gradient.
const auto x_ad = SolveLinearSystem(linear_solver, A_ad, b_ad);
template<typename LinearSolver , typename DerivedA , typename DerivedB >
std::enable_if< internal::is_double_or_symbolic_v< typename DerivedA::Scalar > &&internal::is_double_or_symbolic_v< typename DerivedB::Scalar > &&std::is_same_v< typename DerivedA::Scalar, typename DerivedB::Scalar >, Eigen::Matrix< typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
 Specialized when A and b are both double or symbolic::Expression matrices. More...
 
template<typename LinearSolver , typename DerivedB >
std::enable_if< internal::is_double_or_symbolic_v< typename LinearSolver::MatrixType::Scalar > &&internal::is_double_or_symbolic_v< typename DerivedB::Scalar > &&std::is_same_v< typename LinearSolver::MatrixType::Scalar, typename DerivedB::Scalar >, Eigen::Matrix< typename LinearSolver::MatrixType::Scalar, DerivedB::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedB > &b)
 Specialized when the matrix in linear_solver and b are both double or symbolic::Expression matrices. More...
 
template<typename LinearSolver , typename DerivedB >
std::enable_if< std::is_same_v< typename LinearSolver::MatrixType::Scalar, double > &&internal::is_autodiff_v< typename DerivedB::Scalar >, Eigen::Matrix< typename DerivedB::Scalar, DerivedB::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedB > &b)
 Specialized the matrix in linear_solver is a double-valued matrix and b is an AutoDiffScalar-valued matrix. More...
 
template<typename LinearSolver , typename DerivedA , typename DerivedB >
std::enable_if< std::is_same_v< typename DerivedA::Scalar, double > &&internal::is_autodiff_v< typename DerivedB::Scalar >, Eigen::Matrix< typename DerivedB::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
 Specialized when A is a double-valued matrix and b is an AutoDiffScalar-valued matrix. More...
 
template<typename LinearSolver , typename DerivedA , typename DerivedB >
std::enable_if< internal::is_autodiff_v< typename DerivedA::Scalar >, Eigen::Matrix< typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::ColsAtCompileTime > >::type SolveLinearSystem (const LinearSolver &linear_solver, const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
 Specialized when A is an AutoDiffScalar-valued matrix, and b can contain either AutoDiffScalar or double. More...
 
Get linear solver

Create the linear solver for a given matrix A, which will be used to solve the linear system of equations A * x = b.

The following table indicate the scalar type of the matrix in the returned linear solver, depending on the scalar type in matrix A

A double ADS Expr
solver double double Expr

where ADS stands for Eigen::AutoDiffScalar, and Expr stands for symbolic::Expression. Here is the example code

Eigen::Matrix2d A_val;
A_val << 1, 2, 2, 5;
Eigen::Vector2d b_val(3, 4);
const Eigen::Vector2d x_val =
SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_val), A_val, b_val);
Eigen::Matrix<AutoDiffXd, 2, 2> A_ad;
A_ad(0, 0).value() = A_val(0, 0);
A_ad(0, 0).derivatives() = Eigen::Vector3d(1, 2, 3);
A_ad(0, 1).value() = A_val(0, 1);
A_ad(0, 1).derivatives() = Eigen::Vector3d(2, 3, 4);
A_ad(1, 0).value() = A_val(1, 0);
A_ad(1, 0).derivatives() = Eigen::Vector3d(3, 4, 5);
A_ad(1, 1).value() = A_val(1, 1);
A_ad(1, 1).derivatives() = Eigen::Vector3d(4, 5, 6);
// Solve A * x = b with A containing gradient.
const Eigen::Matrix<AutoDiffXd, 2, 1> x_ad1 =
SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_ad), A_ad, b_val);
Eigen::Matrix<AutoDiffXd, 2, 1> b_ad;
b_ad(0).value() = b_val(0);
b_ad(0).derivatives() = Eigen::Vector3d(5, 6, 7);
b_ad(1).value() = b_val(1);
b_ad(1).derivatives() = Eigen::Vector3d(6, 7, 8);
// Solve A * x = b with b containing gradient.
const Eigen::Matrix<AutoDiffXd, 2, 1> x_ad2 =
SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_val), A_val, b_ad);
// Solve A * x = b with both A and b containing gradient.
const Eigen::Matrix<AutoDiffXd, 2, 1> x_ad3 =
SolveLinearSystem(GetLinearSolver<Eigen::LLT>(A_ad), A_ad, b_ad);

{cc}

template<template< typename, int... > typename LinearSolverType, typename DerivedA >
internal::EigenLinearSolver< LinearSolverType, DerivedA > GetLinearSolver (const Eigen::MatrixBase< DerivedA > &A)
 Get the linear solver for a matrix A. More...
 
solve linear system of equations

Solve linear system of equations A * x = b. Where A is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression, and b is an Eigen matrix of double/AutoDiffScalar/symbolic::Expression. Note that when either A or b contains symbolic::Expression, the other has to contain symbolic::Expression as well. When either A or b contains AutoDiffScalar, we use implicit function theorem to find the gradient in x as ∂x/∂zᵢ = A⁻¹(∂b/∂zᵢ - ∂A/∂zᵢ * x) where z is the variable we take gradient with.

The following table indicate the scalar type of x with A/b containing the specified scalar type. The entries with NA are not supported.

b ＼ A double ADS Expr
double double ADS NA
ADS ADS ADS NA
Expr NA NA Expr

where ADS stands for Eigen::AutoDiffScalar, and Expr stands for symbolic::Expression.

TODO(hongkai.dai): support one of A/b being a double matrix and the other being a symbolic::Expression matrix.

Note
When both A and b are Eigen matrix of double, this function is almost as fast as calling linear_solver.solve(b) directly; when either A or b contains AutoDiffScalar, this function is a lot faster than first instantiating the linear solver of AutoDiffScalar, and then solving the equation with this autodiffable linear solver.
Template Parameters
LinearSolverTypeThe type of linear solver, for example Eigen::LLT. Notice that this is just specifies the solver type (such as Eigen::LLT), not the matrix type (like Eigen::LLT<Eigen::Matrix2d>). All Eigen solvers we care about are templated on the matrix type. Some are further templated on configuration ints. The int... will account for zero or more of these ints, providing a common interface for both types of solvers.
DerivedAAn Eigen Matrix.
DerivedBAn Eigen Vector.
Parameters
AThe matrix A.
bThe vector b.

Here is an example code.

Eigen::Matrix<AutoDiffd<3>, 2, 2> A_ad;
// Set the value and gradient in A_ad with arbitrary values;
Eigen::Matrix2d A_val;
A_val << 1, 2, 3, 4;
// Gradient of A.col(0).
Eigen::Matrix<double, 2, 3> A0_gradient;
A0_gradient << 1, 2, 3, 4, 5, 6;
A_ad.col(0) = InitializeAutoDiff(A_val.col(0), A0_gradient);
// Gradient of A.col(1)
Eigen::Matrix<double, 2, 3> A1_gradient;
A1_gradient << 7, 8, 9, 10, 11, 12;
A_ad.col(1) = InitializeAutoDiff(A_val.col(1), A1_gradient);
// Set the value and gradient of b to arbitrary value.
const Eigen::Vector2d b_val(2, 3);
Eigen::Matrix<double, 2, 3> b_gradient;
b_gradient << 1, 3, 5, 7, 9, 11;
const auto b_ad = InitializeAutoDiff(b_val, b_gradient);
// Solve the linear system A*x=b without the gradient.
const auto x_val = SolveLinearSystem<Eigen::PartialPivLU>(A_val, b_val);
// Solve the linear system A*x=b, together with the gradient.
// x_ad contains both the value of the solution A*x=b, together with its
// gradient.
const auto x_ad = SolveLinearSystem<Eigen::PartialPivLU>(A_ad, b_ad);
template<template< typename, int... > typename LinearSolverType, typename DerivedA , typename DerivedB >
internal::Solution< DerivedA, DerivedB > SolveLinearSystem (const Eigen::MatrixBase< DerivedA > &A, const Eigen::MatrixBase< DerivedB > &b)
 Solves system A*x=b. More...