/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/math/matrix.cpp File Reference

#include "math/matrix.h"
#include <limits>
#include <math.h>
#include "lapack_wrappers.h"
#include "maxdet.h"
#include "matvec3D.h"
#include "debug.h"

Include dependency graph for matrix.cpp:

Go to the source code of this file.

Functions
int	factorizedQPSolver (const Matrix &Qf, const Matrix &Eq, const Matrix &b, const Matrix &InEq, const Matrix &ib, const Matrix &lowerBounds, const Matrix &upperBounds, Matrix &sol, double *objVal)
	Solves a factorized Quadratic Program.
int	linearSolveMPInv (Matrix &A, Matrix &B, Matrix &X)
	Computes a solution of a non-square system A*X = B using Moore-Penrose pseudo-inverse.
int	linearSolveSVD (Matrix &A, Matrix &B, Matrix &X)
	Computes a solution of a non-square system A*X = B using SVD decomposition.
int	LPSolver (const Matrix &Q, const Matrix &Eq, const Matrix &b, const Matrix &InEq, const Matrix &ib, const Matrix &lowerBounds, const Matrix &upperBounds, Matrix &sol, double *objVal)
	Solves a linear program.
void	matrixAdd (const Matrix &L, const Matrix &R, Matrix &M)
	Performs M = L + R.
bool	matrixEqual (const Matrix &R, const Matrix &L)
	Checks if two matrices are identical.
int	matrixInverse (const Matrix &A, Matrix &AInv)
	Computes the inverse of a square matrix.
void	matrixMultiply (const Matrix &L, const Matrix &R, Matrix &M)
	Performs M = L * R.
std::ostream &	operator<< (std::ostream &os, const Matrix &m)
int	QPSolver (const Matrix &Q, const Matrix &Eq, const Matrix &b, const Matrix &InEq, const Matrix &ib, const Matrix &lowerBounds, const Matrix &upperBounds, Matrix &sol, double *objVal)
	Solves a Quadratic Program.
void	testLP ()
	A simple test to check that the LP solver works.
void	testQP ()
	A simple test to check that the QP solver works.
int	triangularSolve (Matrix &A, Matrix &B)
	Solves the system A*X=B with square A. X is overwritten on B.
int	underDeterminedSolveQR (Matrix &A, Matrix &B, Matrix &X)
	Computes minimum norm solution of underdetermined system A*X=B even for rank-deficient A.

---

Function Documentation

int factorizedQPSolver	(	const Matrix &	Qf,
		const Matrix &	Eq,
		const Matrix &	b,
		const Matrix &	InEq,
		const Matrix &	ib,
		const Matrix &	lowerBounds,
		const Matrix &	upperBounds,
		Matrix &	sol,
		double *	objVal
	)

Solves a factorized Quadratic Program.

Solves a quadratic program of the form: minimize solT * QfT * Qf * sol subject to: Eq * sol = b InEq * sol <= ib lowerBounds(i) <= sol(i) <= upperBounds(i) use std::numeric_limits<double>::max() or min() for unbounded variables This is just a transformed version of the general problem above. See Mosek documentation Sec. 7.3.2 for details.

It automatically uses sparse matrices for all the large matrices (not the column vectors) that it needs to create (i.e. ExtEq, ExtInEq and ExtQ).

Definition at line 1019 of file matrix.cpp.

int linearSolveMPInv	(	Matrix &	A,
		Matrix &	B,
		Matrix &	X
	)

Computes a solution of a non-square system A*X = B using Moore-Penrose pseudo-inverse.

Note that solving with the MP pseudo-inverse is brittle, behaves poorly for ill-conditioned matrices. It will return the least squares solution (minimum error norm) for overdetermined systems, but NOT guaranteed to return the minimum norm solution for underdetermined systems.

Definition at line 735 of file matrix.cpp.

int linearSolveSVD	(	Matrix &	A,
		Matrix &	B,
		Matrix &	X
	)

Computes a solution of a non-square system A*X = B using SVD decomposition.

SVD decomposition is more stable than the MP pseudo-inverse. I thikn it will return the least squares solution (minimum error norm) for full-rank overdetermined systems, and the minimum norm solution for full-rank underdetermined systems. NOT sure what it does for non-full rank A.

Definition at line 768 of file matrix.cpp.

int LPSolver	(	const Matrix &	Q,
		const Matrix &	Eq,
		const Matrix &	b,
		const Matrix &	InEq,
		const Matrix &	ib,
		const Matrix &	lowerBounds,
		const Matrix &	upperBounds,
		Matrix &	sol,
		double *	objVal
	)

Solves a linear program.

Solves a linear program of the form: minimize Q * sol subject to: Eq * sol = b InEq * sol <= ib lowerBounds(i) <= sol(i) <= upperBounds(i) use std::numeric_limits<double>::max() or min() for unbounded variables

Return codes: 0 - success > 0 - problem is unfeasible < 0 - error in computation

Definition at line 1154 of file matrix.cpp.

void matrixAdd	(	const Matrix &	L,
		const Matrix &	R,
		Matrix &	M
	)

Performs M = L + R.

Definition at line 688 of file matrix.cpp.

bool matrixEqual	(	const Matrix &	R,
		const Matrix &	L
	)

Checks if two matrices are identical.

Definition at line 700 of file matrix.cpp.

int matrixInverse	(	const Matrix &	A,
		Matrix &	AInv
	)

Computes the inverse of a square matrix.

Inverse is computed with factorization, then triangular solving, both from LAPACK. Returns 0 for success, -1 for unknown failure during computation and 1 if the matrix A is found to be rank-deficient.

Definition at line 923 of file matrix.cpp.

void matrixMultiply	(	const Matrix &	L,
		const Matrix &	R,
		Matrix &	M
	)

Performs M = L * R.

Definition at line 667 of file matrix.cpp.

std::ostream& operator<<	(	std::ostream &	os,
		const Matrix &	m
	)

Definition at line 586 of file matrix.cpp.

int QPSolver	(	const Matrix &	Q,
		const Matrix &	Eq,
		const Matrix &	b,
		const Matrix &	InEq,
		const Matrix &	ib,
		const Matrix &	lowerBounds,
		const Matrix &	upperBounds,
		Matrix &	sol,
		double *	objVal
	)

Solves a Quadratic Program.

Solves a quadratic program of the form: minimize solT * Q * sol subject to: Q symmetric and positive semidefinite Eq * sol = b InEq * sol <= ib lowerBounds(i) <= sol(i) <= upperBounds(i) use std::numeric_limits<double>::max() or min() for unbounded variables

Return codes: 0 - success > 0 - problem is unfeasible < 0 - error in computation

Definition at line 969 of file matrix.cpp.

void testLP

(

)

A simple test to check that the LP solver works.

Test for a linear program: | 1 1 -1 0| |x1| |0| | 3 -1 0 0| |x2| = |0| | 0 1 0 -1| |x3| = |0| |x4|

x2 = 3*x1 x3 = 4*x1 x2 = x4

minimize: | 1 -1 -1 1| |x1| |x2| |x3| |x4| minimize x1 - x2 - x3 - x4 = -6*x1 + x4

x1 <= 5

Definition at line 1213 of file matrix.cpp.

void testQP

(

)

A simple test to check that the QP solver works.

minimize xT | 1 0 | x | 0 4 |

| 1 -1 | x = -4

x1 - x2 = -4

| 1 1 | x <= | 7 | | -1 2 | | -4 |

x1 + x2 <= 7 -x1 +2x2 <= -4

possible solution: [6 -10] According to qpOASES, optimal solution is [-12 -8] with objective 200

Definition at line 1089 of file matrix.cpp.

int triangularSolve	(	Matrix &	A,
		Matrix &	B
	)

Solves the system A*X=B with square A. X is overwritten on B.

Definition at line 714 of file matrix.cpp.

int underDeterminedSolveQR	(	Matrix &	A,
		Matrix &	B,
		Matrix &	X
	)

Computes minimum norm solution of underdetermined system A*X=B even for rank-deficient A.

Definition at line 821 of file matrix.cpp.