replanFunnels_mex.cpp File Reference

#include <mex.h>
#include <matrix.h>
#include <blas.h>
#include <math.h>
#include "drake/util/drakeMexUtil.h"
#include "snopt.hh"
#include "snfilewrapper.hh"
#include <string.h>
#include <memory>
#include <algorithm>
#include "LinearMath/btTransform.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "LinearMath/btTransformUtil.h"

Include dependency graph for replanFunnels_mex.cpp:

Namespaces
	snopt

Functions
double	ptToPolyBullet (double *vertsPr, size_t nRows, size_t nCols, mxArray *normal_vec)
double *	shiftAndTransform (double *verts, double *vertsT, const mxArray *x, mxArray *x0, int k, mxArray *cSk, size_t nRows, size_t nCols)
double	containmentConstraint (snopt::doublereal x_shift[], double *containment_grad)
bool	penetrationCost (snopt::doublereal x[], double *min_dist, double *normal_vec_transformed)
int	snopt_userfun (snopt::integer *Status, snopt::integer *n, snopt::doublereal x[], snopt::integer *needF, snopt::integer *neF, snopt::doublereal F[], snopt::integer *needG, snopt::integer *neG, snopt::doublereal G[], char *cu, snopt::integer *lencu, snopt::integer iu[], snopt::integer *leniu, snopt::doublereal ru[], snopt::integer *lenru)
bool	shiftFunnel_snopt (int funnelIdx, const mxArray *funnelLibrary, const mxArray *obstacles, mwSize numObs, double *min_dist, double *x_opt)
bool	isCollisionFree (int funnelIdx, const mxArray *x, const mxArray *funnelLibrary, const mxArray *obstacles, mwSize numObs, double *min_dist)
bool	isInsideInlet (int funnelIdx, const mxArray *x, const mxArray *funnelLibrary)
void	mexFunction (int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])

Variables
static const mxArray *	funnelLibrary
static int	funnelIdx
static const mxArray *	x_current
static double *	dx_current
static const mxArray *	obstacles
static double	min_dist_snopt
static unique_ptr< snopt::doublereal[]>	rw
static unique_ptr< snopt::integer[]>	iw
static unique_ptr< char[]>	cw
static snopt::integer	lenrw = 0
static snopt::integer	leniw = 0
static snopt::integer	lencw = 0
static btVoronoiSimplexSolver	sGjkSimplexSolver
static btGjkEpaPenetrationDepthSolver	epaSolver
static const double	g_radius = 1.0
static btSphereShape *	g_point = new btSphereShape(g_radius)
const int	DEFAULT_LENRW = 500000
const int	DEFAULT_LENIW = 500000
const int	DEFAULT_LENCW = 500

Function Documentation

double containmentConstraint	(	snopt::doublereal	x_shift[],
		double *	containment_grad
	)

Here is the call graph for this function:

Here is the caller graph for this function:

bool isCollisionFree	(	int	funnelIdx,
		const mxArray *	x,
		const mxArray *	funnelLibrary,
		const mxArray *	obstacles,
		mwSize	numObs,
		double *	min_dist
	)

Here is the call graph for this function:

Here is the caller graph for this function:

bool isInsideInlet	(	int	funnelIdx,
		const mxArray *	x,
		const mxArray *	funnelLibrary
	)

Here is the call graph for this function:

Here is the caller graph for this function:

void mexFunction	(	int	nlhs,
		mxArray *	plhs[],
		int	nrhs,
		const mxArray *	prhs[]
	)

Here is the call graph for this function:

bool penetrationCost	(	snopt::doublereal	x[],
		double *	min_dist,
		double *	normal_vec_transformed
	)

Here is the call graph for this function:

Here is the caller graph for this function:

double ptToPolyBullet	(	double *	vertsPr,
		size_t	nRows,
		size_t	nCols,
		mxArray *	normal_vec
	)

Here is the call graph for this function:

Here is the caller graph for this function:

double* shiftAndTransform	(	double *	verts,
		double *	vertsT,
		const mxArray *	x,
		mxArray *	x0,
		int	k,
		mxArray *	cSk,
		size_t	nRows,
		size_t	nCols
	)

Here is the call graph for this function:

Here is the caller graph for this function:

bool shiftFunnel_snopt	(	int	funnelIdx,
		const mxArray *	funnelLibrary,
		const mxArray *	obstacles,
		mwSize	numObs,
		double *	min_dist,
		double *	x_opt
	)

Here is the call graph for this function:

Here is the caller graph for this function:

int snopt_userfun	(	snopt::integer *	Status,
		snopt::integer *	n,
		snopt::doublereal	x[],
		snopt::integer *	needF,
		snopt::integer *	neF,
		snopt::doublereal	F[],
		snopt::integer *	needG,
		snopt::integer *	neG,
		snopt::doublereal	G[],
		char *	cu,
		snopt::integer *	lencu,
		snopt::integer	iu[],
		snopt::integer *	leniu,
		snopt::doublereal	ru[],
		snopt::integer *	lenru
	)

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

unique_ptr<char[]> cw

static

const int DEFAULT_LENCW = 500

const int DEFAULT_LENIW = 500000

const int DEFAULT_LENRW = 500000

double* dx_current

static

btGjkEpaPenetrationDepthSolver epaSolver

static

int funnelIdx

static

const mxArray* funnelLibrary

static

btSphereShape* g_point = new btSphereShape(g_radius)

static

const double g_radius = 1.0

static

unique_ptr<snopt::integer[]> iw

static

snopt::integer lencw = 0

static

snopt::integer leniw = 0

static

snopt::integer lenrw = 0

static

double min_dist_snopt

static

const mxArray* obstacles

static

unique_ptr<snopt::doublereal[]> rw

static

btVoronoiSimplexSolver sGjkSimplexSolver

static

const mxArray* x_current

static