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

Implements moveBodies, iterateDynamics, and Lemke's algorithm. More...

#include <stdio.h>
#include <list>
#include <vector>
#include <map>
#include <algorithm>
#include "mytools.h"
#include "dynamics.h"
#include "dynJoint.h"
#include "body.h"
#include "robot.h"
#include "contact.h"
#include "world.h"
#include "ivmgr.h"
#include "lapack_wrappers.h"
#include "maxdet.h"
#include "debug.h"

Include dependency graph for dynamics.cpp:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions
void	assembleLCPPrediction (double *lambda, int Arows, int numDOFLimits, std::list< Contact * > *contactList)
void	buildForceTransform (transf &T, vec3 &p, double *transformMat)
void	buildJointConstraints (std::vector< Robot * > &robotVec, std::vector< int > &islandIndices, int numBodies, double *Nu, double *eps, double *H, double *g, double h)
void	fillMatrixBlock (double *B, int ldb, int r1, int c1, int r2, int c2, double *M, int ldm)
int	invertMatrix (int n, double *A, double *INVA)
int	iterateDynamics (std::vector< Robot * > robotVec, std::vector< DynamicBody * > bodyVec, DynamicParameters *dp)
	Computes the new velocites of all bodies, based on contact and joint constraints.
int	moveBodies (int numBodies, std::vector< DynamicBody * > bodyVec, double h)
	Moves all dynamic bodies for one time step.
int	myLemke (double *M, int n, double *q, double *z, bool usePrediction, bool ldbg, int &iterations)
void	printLCPBasis (double *lambda, int Arows, int numDOFLimits, int numContacts)
void	sortVector (int *v, int n)
Variables
FILE *	debugfile
IVmgr *	ivmgr

---

Detailed Description

Implements moveBodies, iterateDynamics, and Lemke's algorithm.

Definition in file dynamics.cpp.

---

Function Documentation

void assembleLCPPrediction	(	double *	lambda,
		int	Arows,
		int	numDOFLimits,
		std::list< Contact * > *	contactList
	)

Attempts to use information from the solution at the previous time step, stored in each contact, to bootstrap the LCP at the current step. In theory, this could help a lot and all the framework is here, we just never got it to actually do much.

Definition at line 1021 of file dynamics.cpp.

void buildForceTransform	(	transf &	T,
		vec3 &	p,
		double *	transformMat
	)

Creates a 6x6 matrix, transformMat, that transforms a wrench expressed in one coordinate system to wrench expressed in another. T is the transform, and p is the new torque origin expressed within the new coordinate system.

Definition at line 101 of file dynamics.cpp.

void buildJointConstraints	(	std::vector< Robot * > &	robotVec,
		std::vector< int > &	islandIndices,
		int	numBodies,
		double *	Nu,
		double *	eps,
		double *	H,
		double *	g,
		double	h
	)

void fillMatrixBlock	(	double *	B,
		int	ldb,
		int	r1,
		int	c1,
		int	r2,
		int	c2,
		double *	M,
		int	ldm
	)

Copies matrix B into a block of matrix M. ldb is the number of rows of B, r1 and r2 are the start and end rows within M, and c1 and c2 are the start and end columns in M. ldm is the number of rows in M.

Definition at line 87 of file dynamics.cpp.

int invertMatrix	(	int	n,
		double *	A,
		double *	INVA
	)

Inverts an n by n matrix.

Definition at line 163 of file dynamics.cpp.

int iterateDynamics	(	std::vector< Robot * >	robotVec,
		std::vector< DynamicBody * >	bodyVec,
		DynamicParameters *	dp
	)

Computes the new velocites of all bodies, based on contact and joint constraints.

This routine builds a Linear Complementarity Problem (LCP) to solve for the velocities of each dynamic body in a given island of connected dynamic bodies (connected by joints or contacts). The robots and bodies in the island are passed to the routine in the vectors robotVec and bodyVec, and the integration time step h is also provided. The useContactEps flag determines whether error correction will be used for the contacts to ensure non-interpenetration. At the end of the routine, the velocity of each body is updated, as well as all contact and joint forces.

Definition at line 322 of file dynamics.cpp.

int moveBodies	(	int	numBodies,
		std::vector< DynamicBody * >	bodyVec,
		double	h
	)

Moves all dynamic bodies for one time step.

Given a vector of pointers to dynamic bodies, and the number of bodies in the vector, this routine will move those bodies in the direction of their current velocity for the length of the timestep, h. It uses the pre-computed velocities and accelerations computed by iterateDynamics and stored for each body.

Definition at line 193 of file dynamics.cpp.

int myLemke	(	double *	M,
		int	n,
		double *	q,
		double *	z,
		bool	usePrediction,
		bool	ldbg,
		int &	iterations
	)

Implementation of Lemke's algorithm for solving pure LCP's. M and q are the matrix and vector that define the LCP. n is the number of rows in M and q, and z is the result. usePrediction is a flag that tells the algorithm to use as a starting basis the initial value of z. If it is set to false, the initial value of z is ignored. ldbg is a debug flag.

Definition at line 1082 of file dynamics.cpp.

void printLCPBasis	(	double *	lambda,
		int	Arows,
		int	numDOFLimits,
		int	numContacts
	)

Definition at line 1042 of file dynamics.cpp.

void sortVector	(	int *	v,
		int	n
	)

Definition at line 1057 of file dynamics.cpp.

---

Variable Documentation

FILE* debugfile

IVmgr* ivmgr