LMIOptimizer Class Reference

Encapsulates the Grasp Force Optimization (GFO) code based on Linear Matrix Inequalitties (LMI). More...

#include <lmiOptimizer.h>

Public Member Functions
int	findOptimalGraspForce ()
	Main GFO routine.
	LMIOptimizer (Grasp *g)
Private Member Functions
void	buildGraspMap ()
	Builds the grasp map computing net object wrench from contact wrenches.
void	buildJacobian ()
	Constructs a version of the hand jacobian.
void	computeNullSpace ()
	GFO routine.
void	computeObjectives ()
	GFO routine.
void	feasibilityAnalysis ()
	GFO routine.
void	lmiFL ()
	Friction LMI helper function.
void	lmiFL (double *lmi, int rowInit, int colInit, int totalRow)
	Friction LMI helper function.
double *	lmiFrictionCones ()
	Friction LMI helper function.
void	lmiPCWF (double cof, double *lmi, int rowInit, int colInit, int totalRow)
	Friction LMI helper function.
void	lmiPWCF ()
	Friction LMI helper function.
void	lmiSFCE ()
	Friction LMI helper function.
void	lmiSFCE (double cof, double cof_t, double *lmi, int rowInit, int colInit, int totalRow)
	Friction LMI helper function.
void	lmiSFCL ()
	Friction LMI helper function.
void	lmiSFCL (double cof, double cof_t, double *lmi, int rowInit, int colInit, int totalRow)
	Friction LMI helper function.
double *	lmiTorqueLimits ()
	Friction LMI helper function.
void	minimalNorm ()
	GFO routine.
void	optm_EffortBarrier ()
	GFO routine.
double *	weightVec ()
	GFO routine.
double *	xzHistoryTransfrom (double *zHistory, int numIters)
	GFO routine.
Private Attributes
double	abstol
double *	c
double	constOffset
double *	extendOptmz0
	Vector of size (m+3), optmz0, as well as the corresponding objective value, duality gap, and number of optimization iteration steps.
double *	extendOptmZHistory
	array of dimension (m+3)*(Number_of_Iterations_at_Optimization_Phase+1), optmZHistory, + 1: the initial feasible grasp force, its objective value, duality gap and number of iterations
double *	externalTorques
	temporary: should come from hand object
double *	F
	These variables are used by maxdet package. Refer to maxdet manual for their explainations.
int *	F_blkszs
int	feasible
	Is the grasp force optimization feasible?
int	feasNTiters
	On entry, the maximum number of total Newton iteraions, and on exit, the real number of Newton Iterations in the feasible phase.
double *	feasXHistory
	array of dimension (NumWrenches+1)*Number_of_Iterations_at_the_Feasibility_Phase, the history of grasp forces x and objective value in the force feasibility phase at the last simulation step.
double *	feasZHistory
	array of dimension (m+3)*Number_of_Iterations_at_the_Feasibility_Phase, the history (iterative values) of z, objective value, duality gap and iteration number in force feasibility phase at each simulation step.
double *	G
int *	G_blkszs
double	gamma
	Termination criteria used in maxdet algorithm. Refer to maxdet manual.
int	GPkRow
int	graspCounter
	Used when saving output from grasp force optimization analysis.
double *	graspMap
	A (6 * numWrenches) matrix containing basis wrenches for each contact vertex (stored in column-major format).
double *	initz0
	Vector of size m, the z value corrsponding to the initial feasible grasp forces computed in force feasibility phase.
double *	Jacobian
	The grasp jacobian.
int	K
int	L
	These variables are used by maxdet package. Refer to maxdet manual for their explainations.
Grasp *	mGrasp
	The grasp that the computations are performed for.
double *	minNormSln
	Min norm solution to $\mbox{GraspMap} * x = F_{ext}$ .
int	negativeFlag
	If it is 1, then terminate the force feasibility phase whenever the objective value becomes negative, i.e. find one valid grasp force. Otherwise, use the regular maxdet termination.
int	nullDim
	Dimension of the null space of the grasp map.
double *	nullSpace
	The null space of the grasp map; (column-major).
int	numDOF
	The number of degrees of freedom of the hand.
int	numWrenches
	Number of basis wrenches for grasp map (3 * numContacts for PCWF).
int	optmNTiters
	The counterpart to feasNTiters.
double *	optmx0
	vector of size (numWrenches+1), the optimal grasp contact forces, computed from optmz0 and the objective value.
double *	optmXHistory
	array of dimension (NumWrenches+1)*(Number_of_Iterations_at_the_Optimization_Phase+1), the counterpart to feasXHistory in the optimization phase at the last step.
double *	optmz0
	Vector of size m, the z value corresponding to optimal grasp forces, computed in force optimization phase.
double *	optmZHistory
double *	optTorques
	vector of size numDOF storing the optimal torque for each DOF
FILE *	pRstFile
	Output file pointer for saving the results of a GFO analysis.
double	reltol
double *	W
double *	Z
Static Private Attributes
static double	GFO_WEIGHT_FACTOR = 1.0
	GFO parameter.

Detailed Description

Encapsulates the Grasp Force Optimization (GFO) code based on Linear Matrix Inequalitties (LMI).

This class contains the first iteration of the GFO code from GraspIt release 0.9. This code is based on the linear matrix inequality technique and was adapted from code written by Li Han and Jeff Trinkle. More information regarding their algorithm can be found in: L. Han, J. Trinkle, and Z. Li, "Grasp Analysis as Linear Matrix Inequality Problems," IEEE Transactions on Robotics and Automation, Vol. 16, No. 6, pp. 663--674, December, 2000.

Note that this functionality is complete, but it has not been throroughly used or tested. Is is also not completely documented, so you will have to read the code and figure it out. Unfortunately, it has fallen into disrepair and has not been kept up-to-date with the rest of GraspIt. As such, it is not used anywhere else, and probably will not work out of the box.

We also have a new way of doing GFO based on Quadratic Programming (QP). That functionality is included with the Grasp class. It is up to date and tested. We recommend to use that version for your GFO code. The LMI version is probably superior from the mathematical standpoint, but the QP version is easier to follow, completely documented and up to date. A good starting point is the function Grasp::computeQuasistaticForcesAndTorques(...)

Note that this file and this version of the GFO code might be removed from future releases of GraspIt.

Definition at line 55 of file lmiOptimizer.h.

Constructor & Destructor Documentation

LMIOptimizer::LMIOptimizer ( Grasp * g ) [inline]

Definition at line 191 of file lmiOptimizer.h.

Member Function Documentation

void LMIOptimizer::buildGraspMap ( ) [private]

Builds the grasp map computing net object wrench from contact wrenches.

Constructs the grasp map matrix. This is a 6 x numWrenches (numWrenches = 3 * mGrasp->numContacts, when each contact is PCWF) matrix that computes the net object wrench given a vector of magnitudes for the contact wrenches.

Definition at line 179 of file lmiOptimizer.cpp.

void LMIOptimizer::buildJacobian ( ) [private]

Constructs a version of the hand jacobian.

Constructs the hand jacobian. It currently assumes there is no palm motion, hence the matrix is numDOF x numWrenches (numWrenches = 3 * mGrasp->numContacts when each contact is PCWF. This jacobian relates joint velocities to contact velocities and its transpose relates contact forces to joint torques.

Distances used to be in meters. Now they are in millimeters.

Definition at line 90 of file lmiOptimizer.cpp.

void LMIOptimizer::computeNullSpace ( ) [private]

GFO routine.

Computes the null space of the grasp map. Adapted from Li Han's code.

Definition at line 219 of file lmiOptimizer.cpp.

void LMIOptimizer::computeObjectives ( ) [private]

GFO routine.

Compute the value of the objective function for each set of z values stores the optimal z value and the corresponding objective value in extendOptmz0.

Definition at line 1047 of file lmiOptimizer.cpp.

void LMIOptimizer::feasibilityAnalysis ( ) [private]

GFO routine.

Solves the question of whether there is a feasible set of contact forces that will result in equilibrium for the object.

Definition at line 828 of file lmiOptimizer.cpp.

int LMIOptimizer::findOptimalGraspForce ( )

Main GFO routine.

Main GFO routine. Adapted from Li Han's code.

Definition at line 1134 of file lmiOptimizer.cpp.

void LMIOptimizer::lmiFL ( ) [private]

Friction LMI helper function.

void LMIOptimizer::lmiFL	(	double *	lmi,
		int	rowInit,
		int	colInit,
		int	totalRow
	)			`[private]`

Friction LMI helper function.

Definition at line 462 of file lmiOptimizer.cpp.

double * LMIOptimizer::lmiFrictionCones ( ) [private]

Friction LMI helper function.

Definition at line 556 of file lmiOptimizer.cpp.

void LMIOptimizer::lmiPCWF	(	double	cof,
		double *	lmi,
		int	rowInit,
		int	colInit,
		int	totalRow
	)			`[private]`

Friction LMI helper function.

Definition at line 469 of file lmiOptimizer.cpp.

void LMIOptimizer::lmiPWCF ( ) [private]

Friction LMI helper function.

void LMIOptimizer::lmiSFCE ( ) [private]

Friction LMI helper function.

void LMIOptimizer::lmiSFCE	(	double	cof,
		double	cof_t,
		double *	lmi,
		int	rowInit,
		int	colInit,
		int	totalRow
	)			`[private]`

Friction LMI helper function.

Definition at line 487 of file lmiOptimizer.cpp.

void LMIOptimizer::lmiSFCL ( ) [private]

Friction LMI helper function.

void LMIOptimizer::lmiSFCL	(	double	cof,
		double	cof_t,
		double *	lmi,
		int	rowInit,
		int	colInit,
		int	totalRow
	)			`[private]`

Friction LMI helper function.

Definition at line 513 of file lmiOptimizer.cpp.

double * LMIOptimizer::lmiTorqueLimits ( ) [private]

Friction LMI helper function.

Adapted from Li Han's Code.

Definition at line 415 of file lmiOptimizer.cpp.

void LMIOptimizer::minimalNorm ( ) [private]

GFO routine.

Find the minimal norm solution to the object wrench equilibirum equation. Adapted from Li Han's Code - I changed the sign of the object wrench.

The solution to the linear equation $G x + F_{ext} = 0$ is $ x = x_0 + V z $ where $x_0 = G^+ * F_{ext}$ is the minimal normal solution.

Grasp map: $G$ .
minNormSln: $x_0$ .
nullSpace: $V$ .

Definition at line 318 of file lmiOptimizer.cpp.

void LMIOptimizer::optm_EffortBarrier ( ) [private]

GFO routine.

Formulate and solve the optimization 4 problem (See the LMI paper)

Definition at line 979 of file lmiOptimizer.cpp.

double * LMIOptimizer::weightVec ( ) [private]

GFO routine.

Definition at line 620 of file lmiOptimizer.cpp.

double * LMIOptimizer::xzHistoryTransfrom	(	double *	zHistory,
		int	numIters
	)			`[private]`

GFO routine.

Transform z values to values of grasp force x.

Definition at line 1105 of file lmiOptimizer.cpp.

Member Data Documentation

double LMIOptimizer::abstol [private]

Definition at line 101 of file lmiOptimizer.h.

double * LMIOptimizer::c [private]

Definition at line 98 of file lmiOptimizer.h.

double LMIOptimizer::constOffset [private]

Definition at line 98 of file lmiOptimizer.h.

double* LMIOptimizer::extendOptmz0 [private]

Vector of size (m+3), optmz0, as well as the corresponding objective value, duality gap, and number of optimization iteration steps.

Definition at line 125 of file lmiOptimizer.h.

double* LMIOptimizer::extendOptmZHistory [private]

array of dimension (m+3)*(Number_of_Iterations_at_Optimization_Phase+1), optmZHistory, + 1: the initial feasible grasp force, its objective value, duality gap and number of iterations

Definition at line 134 of file lmiOptimizer.h.

double* LMIOptimizer::externalTorques [private]

temporary: should come from hand object

Definition at line 92 of file lmiOptimizer.h.

double* LMIOptimizer::F [private]

These variables are used by maxdet package. Refer to maxdet manual for their explainations.

Definition at line 98 of file lmiOptimizer.h.

int * LMIOptimizer::F_blkszs [private]

Definition at line 95 of file lmiOptimizer.h.

int LMIOptimizer::feasible [private]

Is the grasp force optimization feasible?

Definition at line 107 of file lmiOptimizer.h.

int LMIOptimizer::feasNTiters [private]

On entry, the maximum number of total Newton iteraions, and on exit, the real number of Newton Iterations in the feasible phase.

Definition at line 110 of file lmiOptimizer.h.

double* LMIOptimizer::feasXHistory [private]

array of dimension (NumWrenches+1)*Number_of_Iterations_at_the_Feasibility_Phase, the history of grasp forces x and objective value in the force feasibility phase at the last simulation step.

Definition at line 137 of file lmiOptimizer.h.

double* LMIOptimizer::feasZHistory [private]

array of dimension (m+3)*Number_of_Iterations_at_the_Feasibility_Phase, the history (iterative values) of z, objective value, duality gap and iteration number in force feasibility phase at each simulation step.

Definition at line 128 of file lmiOptimizer.h.

double * LMIOptimizer::G [private]

Definition at line 98 of file lmiOptimizer.h.

int * LMIOptimizer::G_blkszs [private]

Definition at line 95 of file lmiOptimizer.h.

double LMIOptimizer::gamma [private]

Termination criteria used in maxdet algorithm. Refer to maxdet manual.

Definition at line 101 of file lmiOptimizer.h.

double LMIOptimizer::GFO_WEIGHT_FACTOR = 1.0 [static, private]

GFO parameter.

Definition at line 59 of file lmiOptimizer.h.

int LMIOptimizer::GPkRow [private]

Definition at line 95 of file lmiOptimizer.h.

int LMIOptimizer::graspCounter [private]

Used when saving output from grasp force optimization analysis.

Definition at line 113 of file lmiOptimizer.h.

double* LMIOptimizer::graspMap [private]

A (6 * numWrenches) matrix containing basis wrenches for each contact vertex (stored in column-major format).

Definition at line 68 of file lmiOptimizer.h.

double* LMIOptimizer::initz0 [private]

Vector of size m, the z value corrsponding to the initial feasible grasp forces computed in force feasibility phase.

Definition at line 116 of file lmiOptimizer.h.

double* LMIOptimizer::Jacobian [private]

The grasp jacobian.

Definition at line 83 of file lmiOptimizer.h.

int LMIOptimizer::K [private]

Definition at line 95 of file lmiOptimizer.h.

int LMIOptimizer::L [private]

These variables are used by maxdet package. Refer to maxdet manual for their explainations.

Definition at line 95 of file lmiOptimizer.h.

Grasp* LMIOptimizer::mGrasp [private]

The grasp that the computations are performed for.

Definition at line 62 of file lmiOptimizer.h.

double* LMIOptimizer::minNormSln [private]

Min norm solution to $\mbox{GraspMap} * x = F_{ext}$ .

Definition at line 86 of file lmiOptimizer.h.

int LMIOptimizer::negativeFlag [private]

If it is 1, then terminate the force feasibility phase whenever the objective value becomes negative, i.e. find one valid grasp force. Otherwise, use the regular maxdet termination.

Definition at line 104 of file lmiOptimizer.h.

int LMIOptimizer::nullDim [private]

Dimension of the null space of the grasp map.

Definition at line 71 of file lmiOptimizer.h.

double* LMIOptimizer::nullSpace [private]

The null space of the grasp map; (column-major).

Definition at line 74 of file lmiOptimizer.h.

int LMIOptimizer::numDOF [private]

The number of degrees of freedom of the hand.

Definition at line 89 of file lmiOptimizer.h.

int LMIOptimizer::numWrenches [private]

Number of basis wrenches for grasp map (3 * numContacts for PCWF).

Definition at line 65 of file lmiOptimizer.h.

int LMIOptimizer::optmNTiters [private]

The counterpart to feasNTiters.

Definition at line 119 of file lmiOptimizer.h.

double* LMIOptimizer::optmx0 [private]

vector of size (numWrenches+1), the optimal grasp contact forces, computed from optmz0 and the objective value.

Definition at line 77 of file lmiOptimizer.h.

double* LMIOptimizer::optmXHistory [private]

array of dimension (NumWrenches+1)*(Number_of_Iterations_at_the_Optimization_Phase+1), the counterpart to feasXHistory in the optimization phase at the last step.

Definition at line 140 of file lmiOptimizer.h.

double* LMIOptimizer::optmz0 [private]

Vector of size m, the z value corresponding to optimal grasp forces, computed in force optimization phase.

Definition at line 122 of file lmiOptimizer.h.

double* LMIOptimizer::optmZHistory [private]

Definition at line 131 of file lmiOptimizer.h.

double* LMIOptimizer::optTorques [private]

vector of size numDOF storing the optimal torque for each DOF

Definition at line 80 of file lmiOptimizer.h.

FILE* LMIOptimizer::pRstFile [private]

Output file pointer for saving the results of a GFO analysis.

Definition at line 143 of file lmiOptimizer.h.

double LMIOptimizer::reltol [private]

Definition at line 101 of file lmiOptimizer.h.

double * LMIOptimizer::W [private]

Definition at line 98 of file lmiOptimizer.h.

double * LMIOptimizer::Z [private]

Definition at line 98 of file lmiOptimizer.h.

The documentation for this class was generated from the following files:

/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/include/lmiOptimizer.h
/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/lmiOptimizer.cpp

LMIOptimizer Class Reference

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation