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

#include "humanHand.h"
#include <QFile>
#include <QTextStream>
#include <list>
#include <Inventor/nodes/SoSphere.h>
#include <Inventor/nodes/SoCylinder.h>
#include <Inventor/nodes/SoMaterial.h>
#include <Inventor/nodes/SoDrawStyle.h>
#include "SoArrow.h"
#include "grasp.h"
#include "matrix.h"
#include "tinyxml.h"
#include "debug.h"
#include "world.h"
#include "profiling.h"

Include dependency graph for humanHand.cpp:

Go to the source code of this file.

Defines
#define	WRAPPER_TOLERANCE   0.995
Functions
Matrix	insPtForceBlockMatrix (unsigned int numInsPoints)
int	LineLineIntersect (vec3 p1, vec3 p2, vec3 p3, vec3 p4, vec3 *pa, vec3 *pb, double *mua, double *mub)
double	pointLineDistance (vec3 p0, vec3 l1, vec3 l2)
	PROF_DECLARE (HH_CONTACT_EQUILIBRIUM)
	PROF_DECLARE (HH_TENDON_EQUILIBRIUM)
	PROF_DECLARE (TENDON_REMOVE_INS_POINT)
	PROF_DECLARE (TENDON_UPDATE_INSERTION_FORCES)
	PROF_DECLARE (TENDON_GET_INS_POINT_FORCE_MAGNITUDES)
	PROF_DECLARE (TENDON_GET_INS_POINT_TRANSFORMS)
	PROF_DECLARE (TENDON_UPDATE_FORCE_INDICATORS)
	PROF_DECLARE (TENDON_COMPUTE_GEOMETRY_CORE)
	PROF_DECLARE (TENDON_UPDATE_GEOMETRY)
	PROF_DECLARE (TENDON_CHECK_INTERSECTIONS)
	PROF_DECLARE (TENDON_REMOVE_INTERSECTIONS)
	PROF_DECLARE (TENDON_REMOVE_TEMP_INSERTION_POINTS)
	PROF_DECLARE (ROTATE_SO_TRANSFORM)
	PROF_DECLARE (LINE_LINE_INTERSECT)
void	rotateSoTransform (SoTransform *tran, vec3 axis, double angle)

---

Define Documentation

#define WRAPPER_TOLERANCE   0.995

Definition at line 45 of file humanHand.cpp.

---

Function Documentation

Matrix insPtForceBlockMatrix

(

unsigned int

numInsPoints

)

Creates the individual force matrices for the "fake contacts" at tendon location insertion points, which are only allowed to apply force in the normal direction (along local z). This should be integrated with the matrices that do the same thing for real contacts.

Definition at line 1167 of file humanHand.cpp.

int LineLineIntersect	(	vec3	p1,
		vec3	p2,
		vec3	p3,
		vec3	p4,
		vec3 *	pa,
		vec3 *	pb,
		double *	mua,
		double *	mub
	)

Definition at line 65 of file humanHand.cpp.

double pointLineDistance	(	vec3	p0,
		vec3	l1,
		vec3	l2
	)

Returns the distance between an infinite line defined by l1 and l2 and a point p0. Modified from http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html

Definition at line 115 of file humanHand.cpp.

PROF_DECLARE

(

HH_CONTACT_EQUILIBRIUM

)

PROF_DECLARE

(

HH_TENDON_EQUILIBRIUM

)

PROF_DECLARE

(

TENDON_REMOVE_INS_POINT

)

PROF_DECLARE

(

TENDON_UPDATE_INSERTION_FORCES

)

Given a level of active and passive forces MAGNITUDES this computes what force is applied at each insertion point, and in what DIRECTION. Geometry needs to be up-to-date (use updateGeometry() ).

PROF_DECLARE

(

TENDON_GET_INS_POINT_FORCE_MAGNITUDES

)

PROF_DECLARE

(

TENDON_GET_INS_POINT_TRANSFORMS

)

PROF_DECLARE

(

TENDON_UPDATE_FORCE_INDICATORS

)

PROF_DECLARE

(

TENDON_COMPUTE_GEOMETRY_CORE

)

PROF_DECLARE

(

TENDON_UPDATE_GEOMETRY

)

Updates the geometry of the connectors between insertion points, which need to move together with the robot's links. However, some of them depend on two links, so we can not use link transforms directly (like we do for the geometry of the insertion points).Rather, this geometry needs to be recomputed after every change in link status.

PROF_DECLARE

(

TENDON_CHECK_INTERSECTIONS

)

Checks if a connector penetrates a cylindrical wrapper by more than the tolerance value. If so, it adds a temporary insertion point on the edge of the cylider. Problems:

• creates a small "jump" in the connector as it goes from being INSIDE the wrapper (acc. to tolerance) to passing through a point ON THE EDGE of the wrapper. This might create discontinuities between time steps for dynamics' numerical integration
• does not allow for lateral "sliding" of the tendon on the wrapper

PROF_DECLARE

(

TENDON_REMOVE_INTERSECTIONS

)

PROF_DECLARE

(

TENDON_REMOVE_TEMP_INSERTION_POINTS

)

PROF_DECLARE

(

ROTATE_SO_TRANSFORM

)

PROF_DECLARE

(

LINE_LINE_INTERSECT

)

Given two line segments, P1-P2 and P3-P4, returns the line segment Pa-Pb that is the shortest route between them. Calculates also the values of mua and mub where Pa = P1 + mua (P2 - P1) Pb = P3 + mub (P4 - P3) Returns FALSE if no solution exists. adapted from http://astronomy.swin.edu.au/~pbourke/geometry/lineline3d/

void rotateSoTransform	(	SoTransform *	tran,
		vec3	axis,
		double	angle
	)

Definition at line 124 of file humanHand.cpp.