A grasp occurs between a hand and an object and has quality measures associated with it. More...
#include <grasp.h>
Signals | |
| void | graspUpdated () |
| Called when contacts have changes and the wrench spaces need to be updated. | |
Public Member Functions | |
| void | accumulateAndDisplayObjectWrenches (std::list< Contact * > *contacts, const Matrix &objectWrenches) |
| Accumulates object wrenches in the external wrench accumulator for the objects. | |
| GWS * | addGWS (const char *type) |
| Adds a GWS of a given type to the grasp, unless one exists already. | |
| void | addProjection (GWSprojection *gp) |
| void | addQM (QualityMeasure *qm) |
| Adds a quality measure to this grasp. | |
| void | collectContacts () |
| Collects all the contacts between the hand and the object in an internal list. | |
| void | collectVirtualContacts () |
| Collects all the virtual contacts on the hand n an internal list. | |
| void | collectVirtualContactsOnObject () |
| Collect all the virtual contacts on the object. | |
| int | computeQuasistaticForces (const Matrix &robotTau) |
| Given a vector of joint torques, computes the contact forces that balance the system. | |
| int | computeQuasistaticForcesAndTorques (Matrix *robotTau, int computation) |
| Computes the contact forces and joint torques that give the most robust equilibrium. | |
| Matrix | contactJacobian (const std::list< Joint * > &joints, const std::list< VirtualContact * > &contacts) |
| A convenience version of the Jacobian function that takes in a list of virtual contacts. | |
| Matrix | contactJacobian (const std::list< Joint * > &joints, const std::list< Contact * > &contacts) |
| A convenience version of the Jacobian function that takes in a list of contacts. | |
| Matrix | contactJacobian (const std::list< Joint * > &joints, const std::list< std::pair< transf, Link * > > &contact_locations) |
| A version of the contact grasp Jacobian, used for GFO routines. | |
| void | displayContactWrenches (std::list< Contact * > *contacts, const Matrix &contactWrenches) |
| Sets local contact wrenches into the contact wrench slots so they can be rendered. | |
| position | getCoG () |
| Returns the c.o.g. used in GWS computations, either from the object of from virtual contacts. | |
| Contact * | getContact (int i) const |
| GWS * | getGWS (const char *type) |
| std::list< Joint * > | getJointsOnContactChains () |
| Gets a list with only the joints on chains that have contacts on them. | |
| double | getMaxRadius () |
| Returns the max radius used in GWS computations, either from the object of from virtual contacts. | |
| void | getMinWrench (double *w) const |
| int | getNumContacts () const |
| int | getNumQM () const |
| GraspableBody * | getObject () const |
| QualityMeasure * | getQM (int which) |
| Returns one of the quality measures that have been associated with this grasp. | |
| Grasp (Hand *h) | |
| bool | isGravitySet () |
| bool | isValid () const |
| void | removeGWS (GWS *gws) |
| Decrements the reference count on a GWS of the given type, and deletes it if the ref count reaches 0. | |
| void | removeProjection (GWSprojection *gp) |
| void | removeQM (int which) |
| Removes a quality measure that has been associated with this grasp. | |
| void | replaceQM (int which, QualityMeasure *qm) |
| Replaces a QM in the list associated with this grasp with another QM. | |
| void | setGravity (bool g) |
| Sets whether QM's should take gravity into account; not very thoroughly tested yet. | |
| void | setMinWrench (double *w) |
| void | setObject (GraspableBody *g) |
| void | setObjectNoUpdate (GraspableBody *g) |
| void | update (std::vector< int > useDimensions=ALL_DIMENSIONS) |
| Collects all the contacts in the internal list and updates the wrench spaces. | |
| void | updateWrenchSpaces (std::vector< int > useDimensions=ALL_DIMENSIONS) |
| Updates (re-computes) the wrench spaces of this grasp and all of their projections. | |
| ~Grasp () | |
Static Public Member Functions | |
| static void | destroyProjection (void *user, SoQtComponent *component) |
| static Matrix | graspMapMatrix (const Matrix &R, const Matrix &D) |
| Computes the grasp map matrix G from friction and normal force matrices R and D. | |
Static Public Attributes | |
| static const std::vector< int > | ALL_DIMENSIONS = std::vector<int>(6, 1) |
| Default option for building a GWS on all 6 dimensions. | |
| static const int | CONTACT_FORCE_EXISTENCE = 0 |
| static const int | CONTACT_FORCE_OPTIMIZATION = 1 |
| static const int | GRASP_FORCE_EXISTENCE = 2 |
| static const int | GRASP_FORCE_OPTIMIZATION = 3 |
Protected Member Functions | |
| double * | getLinkJacobian (int f, int l) |
| Computes the Jacobian of a link wrt the base of the finger, in link coordinates. | |
| void | setRealCentroid (GraspableBody *body) |
| Sets the reference points of all virtual contacts using the c.o.g of the given object. | |
| void | setVirtualCentroid () |
| Sets the reference point that is used for grasp wrench computations as the center of the virtual contacts. | |
| vec3 | virtualCentroid () |
| Computes the virtual center of the internally assembled list of virtual contacts. | |
Protected Attributes | |
| std::vector< Contact * > | contactVec |
| A vector of pointers to the contacts on the object where it touches the hand. | |
| std::list< GWS * > | gwsList |
| A list of pointers to the associated grasp wrench spaces. | |
| Hand * | hand |
| A pointer to the hand that owns this grasp object. | |
| double | minWrench [6] |
| Minimum grasp wrench that can be applied given contact forces that sum to 1. | |
| int | numContacts |
| Number of grasp contacts. | |
| int | numQM |
| Number of quality meausre in the list. | |
| GraspableBody * | object |
| A pointer to the object that is the focus of this grasp. | |
| std::list< GWSprojection * > | projectionList |
| A list of pointer to the associated grasp wrench space projections. | |
| std::list< QualityMeasure * > | qmList |
| A list of pointers to the associated quality measures. | |
| bool | useGravity |
| Tells us if quality metrics should take into account gravity. | |
| bool | valid |
TRUE if the grasp has been updated since the last time the contacts changed | |
Friends | |
| class | LMIOptimizer |
| class | QMDlg |
A grasp occurs between a hand and an object and has quality measures associated with it.
Each hand object has a grasp associated with it, and the grasp occurs between the hand and a graspableBody. Each grasp is incharge of maintaining a set of computed grasp wrench spaces, which are then used by various quality measures. When contacts change between the two, the grasp is updated, meaning each grasp wrench space associated with the grasp is rebuilt, as well as any grasp wrench space projections. All grasp quality measures are also re-evaluated.
The grasp wrench spaces are designed to be shared by different quality metrics. When a new metric is added, if a GWS of the needed type already exists in the list maintained by thhis grasp, the new QM will use it, and a reference count on the GWS is increased. GWS are automatically deleted when no QM that needs them exist anymore.
A grasp can also perform many of the same services on set of VirtualCotacts (see documentation of that class for details). This part however has not neen competely tested, and might produce behavior that is not entirely consistent with how it handles traditional contacts.
This class also provides the mechanism for performing Grasp Force Optimization (GFO) computations using a Quadratic Program (QP) formulation. There are many variants of this computation; some of them are already written. You can generally mix and match between optimization criteria and constraints in any way you want. A good starting point is the computeQuasistaticForcesAndTorques(...) function.
Definition at line 94 of file grasp.h.
| Grasp::Grasp | ( | Hand * | h | ) |
| Grasp::~Grasp | ( | ) |
| void Grasp::accumulateAndDisplayObjectWrenches | ( | std::list< Contact * > * | contacts, | |
| const Matrix & | objectWrenches | |||
| ) |
Accumulates object wrenches in the external wrench accumulator for the objects.
This is a helper function for grasp force optimization routines. Given a set of contacts, and a matrix of contact wrenches expressed in *world* coordinates and relative to object origin (as computed in gfo routines) this function will convert them to object coordinate system and accumulate them in the object's external wrench accumulator. This can serve as an output of the gfo functions and also for rendering purposes, as the external wrench accumulator can then be rendered.
| GWS * Grasp::addGWS | ( | const char * | type | ) |
Adds a GWS of a given type to the grasp, unless one exists already.
Creates a new GWS of type type and adds it to the list of GWS's associated with this grasp if it does not exist already. Current possible types of GWS's are "L1 Norm", and "LInfinity Norm". Returns a pointer to a gws of the requested type that was either created or found.
| void Grasp::addProjection | ( | GWSprojection * | gp | ) |
| void Grasp::addQM | ( | QualityMeasure * | qm | ) |
| void Grasp::collectContacts | ( | ) |
Collects all the contacts between the hand and the object in an internal list.
Gathers the contacts on all links of the hand that are mated with contacts on the grasped object and adds them to the contactVec.
It also collects all contacts from hands attached to this one, and treats all of them as one big grasp. This allows us to do grasp analysis for complex robots, such as the M7, which has manipulators attached at the end of the arms. If you want to do grasp analysis just for the contacts on one of the manipulators, use the instance of Grasp of that particular hand.
| void Grasp::collectVirtualContacts | ( | ) |
Collects all the virtual contacts on the hand n an internal list.
Gathers the virtual contacts on all links of the hand and adds them to the internal list in contactVec. Any GWS computations should then be able to proceed regardless of the fact that these are virtual contacts. However, since we might not have an object, information about the centroid to be used as reference point, and the max radius used for converting torques, are also computed and stored in the virtual contacts themselves.
| void Grasp::collectVirtualContactsOnObject | ( | ) |
Collect all the virtual contacts on the object.
When we are dealing with an object but no hand, gathers the virtual contacts from the object and adds them to the contactVec, doing the same thing as collectContacts() and collectVirtualContacts() do for the hand-and-object and the hand-only cases.
| int Grasp::computeQuasistaticForces | ( | const Matrix & | robotTau | ) |
Given a vector of joint torques, computes the contact forces that balance the system.
One possible version of the GFO problem.
Given a matrix of joint torques applied to the robot joints, this will check if there exists a set of legal contact forces that balance them. If so, it will compute the set of contact forces that adds up to the wrench of least magnitude on the object.
For now, this output of this function is to set the computed contact forces on each contact as dynamic forces, and also to accumulate the resulting wrench on the object in its external wrench accumulator.
Return codes: 0 is success, >0 means finger slip, no legal contact forces exist; <0 means error in computation
| int Grasp::computeQuasistaticForcesAndTorques | ( | Matrix * | robotTau, | |
| int | computation | |||
| ) |
Computes the contact forces and joint torques that give the most robust equilibrium.
This function is the equivalent of the Grasp Force Optimization, but done with the quasi-static formulation cast as a Quadratic Program.
It can perform four types of computation:
There might exist cases of grasps that are reported as form-closed where grasp force existence fails, as this function also asks that the contact forces that balance the object must be possible to apply from actuated DOF's.
For now, this function displays the computed contact forces on the contacts, rather than returning them. It also copies the computed joint forces in the matrix robotTau which is assumed to be large enough for all the joints of the robot and use the robot's numbering scheme.
Return codes: 0 is success, >0 means problem is unfeasible, no equilibrium forces exist; <0 means error in computation
| Matrix Grasp::contactJacobian | ( | const std::list< Joint * > & | joints, | |
| const std::list< VirtualContact * > & | contacts | |||
| ) |
| Matrix Grasp::contactJacobian | ( | const std::list< Joint * > & | joints, | |
| const std::list< std::pair< transf, Link * > > & | contact_locations | |||
| ) |
A version of the contact grasp Jacobian, used for GFO routines.
Computes the contact jacobian J. J relates joint angle motion to contact motion. Its transpose, JT, relates contact forces to joint forces.
The joints and the contacts must both be passed in. Their order in the incoming vectors will determine their indices in the Jacobian. This function will make sure that only the right joints affect each contact: joints that come before the link in contact, on the same chain.
The Jacobian is ALWAYS computed in the local coordinate system of each contact. When multiplied by joint torques, it will thus yield contact forces and torques in the local contact coordinate system of each contact. It is easy to do computations in world coordinates too, but then it is impossible to discards rows that correspond to directions that the contact can not apply force or torque in.
| void Grasp::destroyProjection | ( | void * | user, | |
| SoQtComponent * | component | |||
| ) | [static] |
This is the projection window close callback function. When the projection window is closed, this removes the associated GWSprojection.
| void Grasp::displayContactWrenches | ( | std::list< Contact * > * | contacts, | |
| const Matrix & | contactWrenches | |||
| ) |
Sets local contact wrenches into the contact wrench slots so they can be rendered.
This is a helper function for grasp force optimization routines. Given a set of contacts and a matrix of contact wrenches expressed in *local* contact coordinates, it will set these wrenches in the dynamic wrench slot of the contacts for rendering purposes.
| position Grasp::getCoG | ( | ) |
| Contact* Grasp::getContact | ( | int | i | ) | const [inline] |
| GWS * Grasp::getGWS | ( | const char * | type | ) |
| std::list< Joint * > Grasp::getJointsOnContactChains | ( | ) |
Gets a list with only the joints on chains that have contacts on them.
Retrieve all the joints of the robot, but only if their chains have contacts on them. Joints on chains with no contact have a trivial 0 solution so they only make the problem larger for no good reason. we could go even further and only keep the joints that come *before* the contacts in the chain.
| double * Grasp::getLinkJacobian | ( | int | f, | |
| int | l | |||
| ) | [protected] |
Computes the Jacobian of a link wrt the base of the finger, in link coordinates.
Computes the jacobian for the base frame of link l on finger f relative to the base frame of the finger. This is wrt THE FRAME OF LINK!!!
Distances USED TO BE converted to meters (check why this is...). Now they are kept in MILLIMETERS!
| double Grasp::getMaxRadius | ( | ) |
| int Grasp::getNumContacts | ( | ) | const [inline] |
| int Grasp::getNumQM | ( | ) | const [inline] |
| GraspableBody* Grasp::getObject | ( | ) | const [inline] |
| QualityMeasure * Grasp::getQM | ( | int | which | ) |
Computes the grasp map matrix G from friction and normal force matrices R and D.
Computes the grasp map G. D is the matrix that relates friction edge amplitudes to contact force and R is the matrix that transorms contact forces to world coordinate system. We then need to sum all of them up by multiplication with a summation matrix to get the grasp map
G = S R D
G relates friction edge amplitudes from all contacts to resultant object wrench.
| void Grasp::graspUpdated | ( | ) | [signal] |
Called when contacts have changes and the wrench spaces need to be updated.
Definition at line 81 of file moc_grasp.cpp.
| bool Grasp::isValid | ( | ) | const [inline] |
| void Grasp::removeGWS | ( | GWS * | gws | ) |
| void Grasp::removeProjection | ( | GWSprojection * | gp | ) |
| void Grasp::removeQM | ( | int | which | ) |
| void Grasp::replaceQM | ( | int | which, | |
| QualityMeasure * | qm | |||
| ) |
| void Grasp::setGravity | ( | bool | g | ) | [inline] |
| void Grasp::setObject | ( | GraspableBody * | g | ) | [inline] |
| void Grasp::setObjectNoUpdate | ( | GraspableBody * | g | ) | [inline] |
| void Grasp::setRealCentroid | ( | GraspableBody * | body | ) | [protected] |
| void Grasp::setVirtualCentroid | ( | ) | [protected] |
| void Grasp::update | ( | std::vector< int > | useDimensions = ALL_DIMENSIONS |
) |
Collects all the contacts in the internal list and updates the wrench spaces.
Updates the grasp by collecting all the current contacts on the hand and rebuilding any grasp wrench spaces and their projections. This is usually called after the contacts on the hand have changed.
Also collects and uses any contacts on robots that are attached to this one. See collectContacts() for details.
You can ask for the GWS to be built using only a subset of the 6 dimensions of force and torque normally available. These are, in this order:
fx, fy, fz, tx, ty, tz
If you want only a subset to be used, pass a vector of 6 ints with 1 for the dimensions you want or 0 for those that you do not want. For example, if you want a GWS that only uses fx, fy and tz, pass the following vector:
1, 1, 0, 0, 0, 1
If you want to use all 6 dimensions, pass the default value of Grasp::ALL_DIMENSIONS.
Note that the dimensions along which the GWS is built affects any subsequent projections that are created
| void Grasp::updateWrenchSpaces | ( | std::vector< int > | useDimensions = ALL_DIMENSIONS |
) |
Updates (re-computes) the wrench spaces of this grasp and all of their projections.
See Grasp::update() for details about useDimensions which affect the dimensions that are used to build the GWS.
| vec3 Grasp::virtualCentroid | ( | ) | [protected] |
friend class LMIOptimizer [friend] |
const std::vector< int > Grasp::ALL_DIMENSIONS = std::vector<int>(6, 1) [static] |
Default option for building a GWS on all 6 dimensions.
This replicates GWS::ALL_DIMENSIONS, but it was needed here too so that we don't have to give any caller access to GWS.
const int Grasp::CONTACT_FORCE_EXISTENCE = 0 [static] |
const int Grasp::CONTACT_FORCE_OPTIMIZATION = 1 [static] |
std::vector<Contact *> Grasp::contactVec [protected] |
const int Grasp::GRASP_FORCE_EXISTENCE = 2 [static] |
const int Grasp::GRASP_FORCE_OPTIMIZATION = 3 [static] |
std::list<GWS *> Grasp::gwsList [protected] |
Hand* Grasp::hand [protected] |
double Grasp::minWrench[6] [protected] |
int Grasp::numContacts [protected] |
int Grasp::numQM [protected] |
GraspableBody* Grasp::object [protected] |
std::list<GWSprojection *> Grasp::projectionList [protected] |
std::list<QualityMeasure *> Grasp::qmList [protected] |
bool Grasp::useGravity [protected] |
bool Grasp::valid [protected] |
