void GripperPlugin::leftFingerContactEventHandler(ConstContactsPtr& msg){
if (attaching_mutex.try_lock()){
lock_guard<mutex> lock(attaching_mutex, adopt_lock_t());
for(unsigned int i=0; i < msg->contact_size(); i++){
string name1 = msg->contact(i).collision1();
string name2 = msg->contact(i).collision2();
physics::ModelPtr modelInCollision;
// Parse the collision name to find the link name (approporate GetChildLink accerros not available) This is a hacky way around that.
string collision1ModelName = name1.substr(0, name1.find("::"));
string collision2ModelName = name2.substr(0, name2.find("::"));
if (collision1ModelName.substr(0,2).compare("at")==0) {
modelInCollision = model->GetWorld()->GetModel(collision1ModelName);
leftFingerTargetLink = modelInCollision->GetLink("link");
leftFingerNoContactTime = 0.0f;
return;
} else if (collision2ModelName.substr(0,2).compare("at")==0) {
modelInCollision = model->GetWorld()->GetModel(collision2ModelName);
leftFingerTargetLink = modelInCollision->GetLink("link");
leftFingerNoContactTime = 0.0f;
return;
}
}
if (leftFingerNoContactTime > fingerNoContactThreshold)
leftFingerTargetLink = NULL;
}
}
void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
{
//std::cout<<"CONTACT! "<<std::endl;//<<_contact<<std::endl;
// for all contacts...
for (int i = 0; i < _msg->contact_size(); ++i) {
physics::CollisionPtr collision1 = boost::dynamic_pointer_cast<physics::Collision>(
this->world->GetEntity(_msg->contact(i).collision1()));
physics::CollisionPtr collision2 = boost::dynamic_pointer_cast<physics::Collision>(
this->world->GetEntity(_msg->contact(i).collision2()));
if ((collision1 && !collision1->IsStatic()) && (collision2 && !collision2->IsStatic()))
{
std::string name1 = collision1->GetScopedName();
std::string name2 = collision2->GetScopedName();
//std::cout<<"OBJ CONTACT! "<<name1<<" / "<<name2<<std::endl;
int count = _msg->contact(i).position_size();
// Check to see if the contact arrays all have the same size.
if ((count != _msg->contact(i).normal_size()) ||
(count != _msg->contact(i).wrench_size()) ||
(count != _msg->contact(i).depth_size()))
{
gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n"<<std::endl;
continue;
}
std::string collidingObjName, collidingLink, gripperOfCollidingLink;
physics::CollisionPtr linkCollision;
physics::CollisionPtr objCollision;
physics::Contact contact;
contact = _msg->contact(i);
if (contact.count<1)
{
std::cerr<<"ERROR: GazeboGraspFix: Not enough forces given for contact of ."<<name1<<" / "<<name2<<std::endl;
continue;
}
// all force vectors which are part of this contact
std::vector<gazebo::math::Vector3> force;
// find out which part of the colliding entities is the object, *not* the gripper,
// and copy all the forces applied to it into the vector 'force'.
std::map<std::string,std::string>::const_iterator gripperCollIt = this->collisions.find(name2);
if (gripperCollIt != this->collisions.end())
{ // collision 1 is the object
collidingObjName=name1;
collidingLink=name2;
linkCollision=collision2;
objCollision=collision1;
gripperOfCollidingLink=gripperCollIt->second;
for (int k=0; k<contact.count; ++k)
force.push_back(contact.wrench[k].body1Force);
}
else if ((gripperCollIt=this->collisions.find(name1)) != this->collisions.end())
{ // collision 2 is the object
collidingObjName=name2;
collidingLink=name1;
linkCollision=collision1;
objCollision=collision2;
gripperOfCollidingLink=gripperCollIt->second;
for (int k=0; k<contact.count; ++k)
force.push_back(contact.wrench[k].body2Force);
}
gazebo::math::Vector3 avgForce;
// compute average/sum of the forces applied on the object
for (int k=0; k<force.size(); ++k){
avgForce+=force[k];
}
avgForce/=force.size();
gazebo::math::Vector3 avgPos;
// compute center point (average pose) of all the origin positions of the forces appied
for (int k=0; k<contact.count; ++k) avgPos+=contact.positions[k];
avgPos/=contact.count;
// now, get average pose relative to the colliding link
gazebo::math::Pose linkWorldPose=linkCollision->GetLink()->GetWorldPose();
// To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
gazebo::math::Matrix4 worldToLink=linkWorldPose.rot.GetAsMatrix4();
worldToLink.SetTranslate(linkWorldPose.pos);
gazebo::math::Matrix4 worldToContact=gazebo::math::Matrix4::IDENTITY;
//we can assume that the contact has identity rotation because we don't care about its orientation.
//We could always set another rotation here too.
worldToContact.SetTranslate(avgPos);
// now, worldToLink * contactInLocal = worldToContact
// hence, contactInLocal = worldToLink.Inv * worldToContact
gazebo::math::Matrix4 worldToLinkInv = worldToLink.Inverse();
gazebo::math::Matrix4 contactInLocal = worldToLinkInv * worldToContact;
gazebo::math::Vector3 contactPosInLocal = contactInLocal.GetTranslation();
//std::cout<<"---------"<<std::endl;
//std::cout<<"CNT in loc: "<<contactPosInLocal.x<<","<<contactPosInLocal.y<<","<<contactPosInLocal.z<<std::endl;
/*gazebo::math::Vector3 sDiff=avgPos-linkWorldPose.pos;
std::cout<<"SIMPLE trans: "<<sDiff.x<<","<<sDiff.y<<","<<sDiff.z<<std::endl;
std::cout<<"coll world pose: "<<linkWorldPose.pos.x<<", "<<linkWorldPose.pos.y<<", "<<linkWorldPose.pos.z<<std::endl;
std::cout<<"contact avg pose: "<<avgPos.x<<", "<<avgPos.y<<", "<<avgPos.z<<std::endl;
gazebo::math::Vector3 lX=linkWorldPose.rot.GetXAxis();
gazebo::math::Vector3 lY=linkWorldPose.rot.GetYAxis();
gazebo::math::Vector3 lZ=linkWorldPose.rot.GetZAxis();
std::cout<<"World ori: "<<linkWorldPose.rot.x<<","<<linkWorldPose.rot.y<<","<<linkWorldPose.rot.z<<","<<linkWorldPose.rot.w<<std::endl;
std::cout<<"x axis: "<<lX.x<<","<<lX.y<<","<<lX.z<<std::endl;
std::cout<<"y axis: "<<lY.x<<","<<lY.y<<","<<lY.z<<std::endl;
std::cout<<"z axis: "<<lZ.x<<","<<lZ.y<<","<<lZ.z<<std::endl;*/
// now, get the pose of the object and compute it's relative position to the collision surface.
gazebo::math::Pose objWorldPose = objCollision->GetLink()->GetWorldPose();
gazebo::math::Matrix4 worldToObj = objWorldPose.rot.GetAsMatrix4();
worldToObj.SetTranslate(objWorldPose.pos);
gazebo::math::Matrix4 objInLocal = worldToLinkInv * worldToObj;
gazebo::math::Vector3 objPosInLocal = objInLocal.GetTranslation();
{
boost::mutex::scoped_lock lock(this->mutexContacts);
CollidingPoint& p = this->contacts[collidingObjName][collidingLink]; // inserts new entry if doesn't exist
p.gripperName=gripperOfCollidingLink;
p.collLink=linkCollision;
p.collObj=objCollision;
p.force+=avgForce;
p.pos+=contactPosInLocal;
p.objPos+=objPosInLocal;
p.sum++;
}
//std::cout<<"Average force of contact= "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" out of "<<force.size()<<" vectors."<<std::endl;
}
}
}
