A contact that exists even when a hand is not perfectly touching another object. More...
#include <contact.h>
Public Member Functions | |
void | computeWrenches (bool useObjectData=false, bool simply=false) |
Wrench computation is done in world coordinates considers the fact that we have no object. | |
position | getCenter () |
Gets the center that is used in wrench computations. | |
int | getFingerNum () |
Returns the number of the finger that this contact is on. | |
int | getLinkNum () |
Returns the number of the link that this contact is on. | |
double | getMaxRadius () |
Gets the max radius that is used in wrench computations. | |
void | getObjectDistanceAndNormal (Body *body, vec3 *objDistance, vec3 *objNormal) |
Computes the distance to the object along the contact normal and the object surface normal at the closest point. | |
SoSeparator * | getVisualIndicator () |
The visual indicator is just a thin red cylinder. | |
void | getWorldIndicator (bool useObjectData=false) |
Another indicator that can be atatched directly to the world root, for debug. | |
position | getWorldLocation () |
The location of this contact in world coordinates. | |
vec3 | getWorldNormal () |
The normal of this contact (pointing outwards) in world coordinates. | |
void | mark (bool m) |
Changes the visual marker to show that this contact is marked; for debug purposes. | |
void | readFromFile (FILE *fp) |
Loads this contact, including friction edges, from a file. | |
void | scaleWrenches (double factor) |
Scales the wrench space of this contact by a given factor. | |
void | setBody (Body *b) |
Sets the body that this contact is on, presumably to a robot link. | |
void | setCenter (position c) |
Sets the c.o.g that is to be used in wrench computations. | |
void | setObject (Body *b) |
Sets an object that this contact is not exactly on, but we use in calculations. | |
void | setRadius (float r) |
Sets the max radius that is to be used in wrench computations. | |
int | setUpFrictionEdges (bool dynamicsOn=false) |
The virtual contact can not set up its own friction edges. | |
void | updateContact (Body *object) |
Updates mObjectDistance and mObjectNormal based on current world locations. | |
VirtualContact (const VirtualContact *original) | |
Copy constructor copies friction edges and location. | |
VirtualContact (int f, int l, Contact *original) | |
Also flips the normal of the original contact, to make it point outward. | |
VirtualContact () | |
The constructor initializes an empty virtual contact, setting it as its own mate. | |
void | writeToFile (FILE *fp) |
Writes this contact, including friction edges, to a file. | |
~VirtualContact () | |
Destructor. | |
Private Member Functions | |
void | init () |
Empty contact initialization shared by all constructors; sets the contact as its own mate. | |
Private Attributes | |
position | mCenter |
Meant to replace the object CoG when a virtual contact has no object. | |
int | mFingerNum |
Which finger (kinematic chain) of the robot this virtual contact is on. -1 means the palm. | |
int | mLinkNum |
Which link of the chain this virtual contact is on. | |
float | mMaxRadius |
Meant to replace the object max radius when a virtual contact has no object. | |
SoSeparator * | mWorldInd |
We can hook this up directly to the world root to see the contact; for debug purposes. | |
SoMaterial * | mZaxisMat |
We keep a pointer so we can change this contact's appearance on the fly; for debug. |
A contact that exists even when a hand is not perfectly touching another object.
This class is meant for studing grasp quality when there is really no grasp, as the hand is not exactly touching an object. The high-level purpose is to convert the grasp quality function to a continous function that exists in all hand configurations, rather then a highly discrete function that only is non-zero when the hand is touching an object in the right way. However, this line of work is not completed and this class is in bad need of an overhaul.
A virtual contact does NOT come in a pair - there is no mate. As such, any instance of this will have the mate set to NULL, a distinct sign of a virtual contact. Also, virtual contacts have the normals pointing outward, instead of the usual contacts whose normals point inward. This is confusing and should probably be changed at some point.
The VirtualContact does not always need a body2. In fact, most of its design can work if the body2 is NULL. However, it can have a body2, in which case some distance functions are computed based on the body2. This is not very well implemented right now and subject to change.
A virtual contact can be instantiated from any kind of contact as it pretty much only copies the friction edges and location. It also needs a separate computeWrenches to eliminate the need for the object.
All computations for a virtual contact (like wrenches, etc) are performed in the world coordinate system, unlike the usual contact which is in the object's coordinate system. The reason is that we can have a grasp defined my many virtual contacts on the hand (rather then meny traditional contacts on an object). In this case, each link of the hand has its own coordinate frame, so in order to have a coherent frame for the grasp we use world coordinates.
Definition at line 473 of file contact.h.
VirtualContact::VirtualContact | ( | ) |
The constructor initializes an empty virtual contact, setting it as its own mate.
Just calls the super and then calls init to give default values to the virtual contact specific parameters.
Definition at line 1147 of file contact.cpp.
VirtualContact::VirtualContact | ( | int | f, | |
int | l, | |||
Contact * | original | |||
) |
Also flips the normal of the original contact, to make it point outward.
When copying from a regular contact, we must be careful because the normal of a virtual contact points outwards, whereas the normal of a trasitional contact points inwards.
Definition at line 1179 of file contact.cpp.
VirtualContact::VirtualContact | ( | const VirtualContact * | original | ) |
Copy constructor copies friction edges and location.
Copies the location, friction edges, and coefficients of the original. This should really use a super's copy constructor, but we never wrote one. Does not copy the wrenches; the new contact needs to build them itself from the friction edges.
Definition at line 1157 of file contact.cpp.
VirtualContact::~VirtualContact | ( | ) |
Destructor.
Just sets the mate to NULL and let the super destructor take care of the rest.
Definition at line 1203 of file contact.cpp.
void VirtualContact::computeWrenches | ( | bool | useObjectData = false , |
|
bool | simplify = false | |||
) |
Wrench computation is done in world coordinates considers the fact that we have no object.
This computes the wrenches of the virtual contact as used for grasp quality computations. The important aspect to take into account is wether we are using an object or not.
If we are using an object, we assume that contact centroid and maxradius have already been set to match those of the object (hopefully, the grasp has done this). Also, the force applied be the contact is scaled by a function of the distance between the contact and the actual object.
If there is no object, we assume that contact centroid and maxradius have been preset dependign only on the set of virtual contacts that make up the grasp (again, we hope the grasp has done this). There is no scaling involved, all forces are handled normally as if the contact was actually on the object.
Definition at line 1250 of file contact.cpp.
position VirtualContact::getCenter | ( | ) | [inline] |
int VirtualContact::getFingerNum | ( | ) | [inline] |
int VirtualContact::getLinkNum | ( | ) | [inline] |
double VirtualContact::getMaxRadius | ( | ) | [inline] |
void VirtualContact::getObjectDistanceAndNormal | ( | Body * | body, | |
vec3 * | objDistance, | |||
vec3 * | objNormal | |||
) |
Computes the distance to the object along the contact normal and the object surface normal at the closest point.
Sets objDistance to be the vector from the contact to the closest point on the given body. Sets objNormal to be the object surface normal at that point.
Definition at line 1542 of file contact.cpp.
SoSeparator * VirtualContact::getVisualIndicator | ( | ) | [virtual] |
The visual indicator is just a thin red cylinder.
Reimplemented from Contact.
Definition at line 1402 of file contact.cpp.
void VirtualContact::getWorldIndicator | ( | bool | useObjectData = false |
) |
Another indicator that can be atatched directly to the world root, for debug.
Gives us a visual indicator of what this contact looks like, in WORLD COORDINATES. Since this contact has to be transformed to world coordinates for the sake of grasp analysis, this allows us to be sure that the transformation makes sense.
It assumes WRENCHES have been computed.
Definition at line 1330 of file contact.cpp.
position VirtualContact::getWorldLocation | ( | ) |
The location of this contact in world coordinates.
Definition at line 1209 of file contact.cpp.
vec3 VirtualContact::getWorldNormal | ( | ) |
The normal of this contact (pointing outwards) in world coordinates.
Definition at line 1215 of file contact.cpp.
void VirtualContact::init | ( | ) | [private] |
Empty contact initialization shared by all constructors; sets the contact as its own mate.
Initializes an empty (and for now unusable) contact. Sets it as its own mate, which is the distinctive sign of a virtual contact.
Definition at line 1133 of file contact.cpp.
void VirtualContact::mark | ( | bool | m | ) |
Changes the visual marker to show that this contact is marked; for debug purposes.
Changes the color of the visual indicator from red to blue if this contact is to be "marked".
Definition at line 1445 of file contact.cpp.
void VirtualContact::readFromFile | ( | FILE * | fp | ) |
Loads this contact, including friction edges, from a file.
Loads all the info for this contact from a file previously written by VirtualContact::writeToFile(...)
Reimplemented in VirtualContactOnObject.
Definition at line 1497 of file contact.cpp.
void VirtualContact::scaleWrenches | ( | double | factor | ) |
Scales the wrench space of this contact by a given factor.
Scales the force vector based on how distant the VirtualContact is from the actual object. The intuition is as follows: we do not want Grasp Quality to be a step function depending wether a contact is on of off the surface of the object. Ratherm we want it smooth so we allow a VirtualContact to contribute to GQ even if it's a bit off the surface of the object.
Definition at line 1313 of file contact.cpp.
void VirtualContact::setBody | ( | Body * | b | ) | [inline] |
void VirtualContact::setCenter | ( | position | c | ) | [inline] |
void VirtualContact::setObject | ( | Body * | b | ) | [inline] |
void VirtualContact::setRadius | ( | float | r | ) | [inline] |
int VirtualContact::setUpFrictionEdges | ( | bool | dynamicsOn = false |
) | [virtual] |
The virtual contact can not set up its own friction edges.
The virtual contact, for now, does not compute its own friction edges, but just inherits them from the regular contact that it starts from. Alternatively, if it loaded from a file, it reads them in from the file.
In the future, this hierarchy needs to be better engineered.
Implements Contact.
Definition at line 1228 of file contact.cpp.
void VirtualContact::updateContact | ( | Body * | object | ) |
Updates mObjectDistance and mObjectNormal based on current world locations.
void VirtualContact::writeToFile | ( | FILE * | fp | ) |
Writes this contact, including friction edges, to a file.
Saves all the info needed for this contact (what body and link it's on, location, normal, friction edges and coefficient) to a file.
Reimplemented in VirtualContactOnObject.
Definition at line 1462 of file contact.cpp.
position VirtualContact::mCenter [private] |
int VirtualContact::mFingerNum [private] |
int VirtualContact::mLinkNum [private] |
float VirtualContact::mMaxRadius [private] |
SoSeparator* VirtualContact::mWorldInd [private] |
SoMaterial* VirtualContact::mZaxisMat [private] |