/*! 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. */ void Grasp::accumulateAndDisplayObjectWrenches(std::list<Contact*> *contacts, const Matrix &objectWrenches) { int count = 0; std::list<Contact*>::iterator it; for (it=contacts->begin(); it!=contacts->end(); it++, count++) { Contact *contact = *it; vec3 force(objectWrenches.elem(6*count+0,0), objectWrenches.elem(6*count+1,0), objectWrenches.elem(6*count+2,0)); vec3 torque(objectWrenches.elem(6*count+3,0), objectWrenches.elem(6*count+4,0), objectWrenches.elem(6*count+5,0)); if (contact->getBody2()->isDynamic()) { DynamicBody *object = (DynamicBody*)(contact->getBody2()); //compute force and torque in body reference frame //and scale them down for now for rendering and output purposes force = 1.0e-6 * force * object->getTran().inverse(); //torque is also scaled by maxRadius in conversion matrix torque = 1.0e-6 * torque * object->getTran().inverse(); //accumulate them on object object->addForce(force); object->addTorque(torque); } } }
/*! 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. */ void Grasp::displayContactWrenches(std::list<Contact*> *contacts, const Matrix &contactWrenches) { int count = 0; std::list<Contact*>::iterator it; for (it=contacts->begin(); it!=contacts->end(); it++, count++) { Contact *contact = *it; if (contact->getBody2()->isDynamic()) { //wrench is also scaled down for now for rendering and output purposes //we also transform the wrench to the mate's coordinate system, which //usually involves negating the x and z axes double dynWrench[6]; for (int i=0; i<6; i++) { dynWrench[i] = -1.0 * 1.0e-6 * contactWrenches.elem(6*count+i,0); } //the y axis is not negated dynWrench[1] = -1.0 * dynWrench[1]; dynWrench[4] = -1.0 * dynWrench[4]; contact->getMate()->setDynamicContactWrench(dynWrench); } } }