Gazebo magnet simulation

edit

This patch was for an older version of Gazebo, but should be what you need to get started. This patch permits a gripper to be used to turn the magnet on, and whatever is connected that isn't already connected will attach itself (I do not recall if only the first body or any body that's connected).

Hopefully you can complete the ROS Gazebo changes and add it here. Good luck!

Index: server/sensors/contact/ContactSensor.hh
===================================================================
--- server/sensors/contact/ContactSensor.hh (revision 7322)
+++ server/sensors/contact/ContactSensor.hh (working copy)
@@ -62,12 +62,18 @@
     /// \brief Return the number of contacts
     public: unsigned int GetContactCount() const;

+    /// \brief Return the sensor's body
+    public: Body* GetBody() const;
+
     /// \brief Get a contact time
     public: double GetContactTime(unsigned int index) const;

     /// \brief Return a contact state
     public: uint8_t GetContactState(unsigned int index) const;

+    /// \brief Return a contact GeomID
+    public: Body* GetContactBody(unsigned int index) const;
+
     /// \brief Reset the contact states
     public: void ResetContactStates();

@@ -96,6 +102,8 @@
     private: uint8_t *contactStates;
     private: double *contactTimes;
     private: unsigned int contactCount;
+    private: Body**  contactBody; //added by gte
+
   };
   /// \}
   /// \}
Index: server/sensors/contact/ContactSensor.cc
===================================================================
--- server/sensors/contact/ContactSensor.cc (revision 7322)
+++ server/sensors/contact/ContactSensor.cc (working copy)
@@ -37,6 +37,7 @@
 #include "Simulator.hh"
 #include "SensorFactory.hh"
 #include "Geom.hh"
+#include "Body.hh"
 #include "ContactParams.hh"
 #include "ContactSensor.hh"

@@ -57,6 +58,7 @@

   this->contactCount = 0;
   this->contactStates = NULL;
+  this->contactBody = NULL; 
 }


@@ -69,6 +71,9 @@
   if (this->contactStates)
     delete [] this->contactStates;

+  if (this->contactBody)  
+    delete [] this->contactBody;
+
   if (this->contactTimes)
     delete [] this->contactTimes;

@@ -90,6 +95,16 @@
 }

 //////////////////////////////////////////////////////////////////////////////
+/// Get the contact Geom
+Body * ContactSensor::GetContactBody(unsigned int index) const
+{
+  if (index < this->contactCount)
+    return this->contactBody[ index ];
+
+  return 0;
+}
+
+//////////////////////////////////////////////////////////////////////////////
 /// Return the number of contacts
 unsigned int ContactSensor::GetContactCount() const
 {
@@ -97,6 +112,13 @@
 }

 //////////////////////////////////////////////////////////////////////////////
+/// Return the number of contacts
+Body* ContactSensor::GetBody() const
+{
+  return this->body;
+}
+
+//////////////////////////////////////////////////////////////////////////////
 /// Return the contact states
 uint8_t ContactSensor::GetContactState(unsigned int index) const
 {
@@ -111,6 +133,7 @@
 void ContactSensor::ResetContactStates()
 {
   memset(this->contactStates, 0, sizeof(uint8_t) * this->contactCount);
+  memset(this->contactBody, 0, sizeof(Body*) * this->contactCount); 
 }

 //////////////////////////////////////////////////////////////////////////////
@@ -138,9 +161,11 @@
   this->contactCount = this->geomNamesP.size();
   this->contactTimes = new double[ this->contactCount ];
   this->contactStates = new uint8_t[ this->contactCount ];
+  this->contactBody = new Body* [ this->contactCount ];  

+
   memset(this->contactStates,0, sizeof(uint8_t) * this->contactCount);
-  memset(this->contactStates,0, sizeof(double) * this->contactCount);
+//  memset(this->contactStates,0, sizeof(double) * this->contactCount);
 }

 //////////////////////////////////////////////////////////////////////////////
@@ -197,8 +222,34 @@
 {
   std::vector< ParamT<std::string> *>::iterator iter;
   int i = 0;
+  for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); 
+       iter++, i++)
+  {
+    if ( **(*iter) == g1->GetName())
+    {
+      this->contactStates[i] = 1;
+      this->contactBody[i] = (Body*)g2->GetParent(); 
+      this->contactTimes[i] = Simulator::Instance()->GetRealTime();
+    }else if (**(*iter) == g2->GetName() )
+    {
+      this->contactStates[i] = 1;
+      this->contactBody[i] = (Body*)g1->GetParent(); 
+      this->contactTimes[i] = Simulator::Instance()->GetRealTime();

+    }
+  }
+}

+
+/*
+//////////////////////////////////////////////////////////////////////////////
+/// Contact callback
+void ContactSensor::ContactCallback(Geom *g1, Geom *g2)
+{
+  std::vector< ParamT<std::string> *>::iterator iter;
+  int i = 0;
+
+
   for (iter = this->geomNamesP.begin(); iter != this->geomNamesP.end(); 
        iter++, i++)
   {
@@ -209,4 +260,4 @@
     }
   }

-}
+}*/
Index: server/controllers/contactMagnet/SConscript
===================================================================
--- server ...