Commit c79a1912 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added OnAttach() and OnDetach() to grasp fix plugin

parent 2383a1f9
...@@ -81,9 +81,19 @@ public: ...@@ -81,9 +81,19 @@ public:
GazeboGraspFix(); GazeboGraspFix();
GazeboGraspFix(physics::ModelPtr _model); GazeboGraspFix(physics::ModelPtr _model);
virtual ~GazeboGraspFix(); virtual ~GazeboGraspFix();
/**
* Gets called just after the object has been attached to the link
*/
virtual void OnAttach(const std::string& objectName){}
/**
* Gets called just after the object has been detached to the link
*/
virtual void OnDetach(const std::string& objectName){}
private:
virtual void Init(); virtual void Init();
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
private:
/** /**
* Collects for each object all forces which are currently applied on it. * Collects for each object all forces which are currently applied on it.
* Then, for each object, checks whether of all the forces applied, * Then, for each object, checks whether of all the forces applied,
......
...@@ -617,6 +617,7 @@ bool GazeboGraspFix::HandleAttach(const std::string& objName) ...@@ -617,6 +617,7 @@ bool GazeboGraspFix::HandleAttach(const std::string& objName)
} }
gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - this->palmLink->GetWorldPose(); gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() - this->palmLink->GetWorldPose();
this->palmLink->AttachStaticModel(obj,diff); this->palmLink->AttachStaticModel(obj,diff);
this->OnAttach(objName);
#else #else
physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(this->world->GetEntity(objName)); physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(this->world->GetEntity(objName));
if (!obj.get()){ if (!obj.get()){
...@@ -633,6 +634,7 @@ bool GazeboGraspFix::HandleAttach(const std::string& objName) ...@@ -633,6 +634,7 @@ bool GazeboGraspFix::HandleAttach(const std::string& objName)
// it, the fingers keep wobbling, which can create difficulties when moving the arm. // it, the fingers keep wobbling, which can create difficulties when moving the arm.
obj->GetLink()->SetCollideMode("none"); obj->GetLink()->SetCollideMode("none");
} }
this->OnAttach(objName);
#endif // USE_MODEL_ATTACH #endif // USE_MODEL_ATTACH
return true; return true;
} }
...@@ -646,6 +648,7 @@ void GazeboGraspFix::HandleDetach(const std::string& objName) ...@@ -646,6 +648,7 @@ void GazeboGraspFix::HandleDetach(const std::string& objName)
return; return;
} }
this->palmLink->DetachStaticModel(objName); this->palmLink->DetachStaticModel(objName);
this->OnDetach(objName);
#else #else
physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(this->world->GetEntity(objName)); physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>(this->world->GetEntity(objName));
if (!obj.get()){ if (!obj.get()){
...@@ -657,5 +660,6 @@ void GazeboGraspFix::HandleDetach(const std::string& objName) ...@@ -657,5 +660,6 @@ void GazeboGraspFix::HandleDetach(const std::string& objName)
obj->GetLink()->SetCollideMode("all"); obj->GetLink()->SetCollideMode("all");
} }
this->fixedJoint->Detach(); this->fixedJoint->Detach();
this->OnDetach(objName);
#endif // USE_MODEL_ATTACH #endif // USE_MODEL_ATTACH
} }
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment