Elec lead

examples/dave_demo_launch/launch/dave_electrical_mate.launch

@@ -28,9 +28,9 @@
 
     <include file="$(find rexrov_description)/launch/upload_rexrov_oberon7.launch">
         <arg name="namespace" value="rexrov"/>
-        <arg name="x" value="7.8"/>
-        <arg name="y" value="-0.2"/>
-        <arg name="z" value="-93.7"/>
+        <arg name="x" value="4.0"/>
+        <arg name="y" value="3.0"/>
+        <arg name="z" value="-97.5"/>
         <arg name="yaw" value="3.141592"/>
     </include>
 

examples/dave_demo_launch/launch/dave_plug_and_socket_demo.launch

@@ -28,9 +28,9 @@
 
     <include file="$(find rexrov_description)/launch/upload_rexrov_oberon7.launch">
         <arg name="namespace" value="rexrov"/>
-        <arg name="x" value="7.5"/>
-        <arg name="y" value="1.25"/>
-        <arg name="z" value="-95.5"/>
+        <arg name="x" value="3.5"/>
+        <arg name="y" value="2.5"/>
+        <arg name="z" value="-96.5"/>
         <arg name="yaw" value="3.141592"/>
     </include>
 
@@ -46,6 +46,6 @@
 
   <include file="$(find plug_and_socket_description)/launch/upload_socket_platform.launch"/>
   <node name="spawn_socket" pkg="gazebo_ros" type="spawn_model"
-        args="-urdf -param socket_platform -model socket_platform -x 0 -y 0 -z -99 -Y -1.57079632679" respawn="false" output="screen" />
+        args="-urdf -param socket_platform -model socket_platform -x 0 -y 0 -z -100 -Y -1.57079632679" respawn="false" output="screen" />
 
 </launch>

gazebo/dave_gazebo_model_plugins/include/dave_gazebo_model_plugins/plug_and_socket_plugin.h

@@ -26,6 +26,9 @@
 #include <string>
 #include <vector>
 
+#include <ros/ros.h>
+#include <geometry_msgs/Vector3Stamped.h>
+
 #include <gazebo/common/common.hh>
 #include <gazebo/gazebo.hh>
 #include <gazebo/physics/physics.hh>
@@ -37,79 +40,85 @@ namespace gazebo
   class PlugAndSocketMatingPlugin : public ModelPlugin
   {
     /// \brief Pointer to the Gazebo world.
-    private: physics::WorldPtr world;
+    protected: physics::WorldPtr world;
 
     /// \brief Name of the socket model
-    private: std::string socketModelName;
+    protected: std::string socketModelName;
 
     /// \brief Pointer to the socket model.
-    private: physics::ModelPtr socketModel;
+    protected: physics::ModelPtr socketModel;
 
     /// \brief Name of the socket tube link
-    private: std::string tubeLinkName;
+    protected: std::string tubeLinkName;
 
     /// \brief Pointer to the hollow tube like structure that receives the plug
-    private: physics::LinkPtr tubeLink;
+    protected: physics::LinkPtr tubeLink;
 
     /// \brief Name of the sensor plate link name
-    private: std::string sensorPlateName;
+    protected: std::string sensorPlateName;
 
     /// \brief Pointer to the sensor plate in the socket model that senses when
     /// the plug is inserter.
-    private: physics::LinkPtr sensorPlate;
+    protected: physics::LinkPtr sensorPlate;
 
     /// \brief Model name of the plub model
-    private: std::string plugModelName;
+    protected: std::string plugModelName;
 
     /// \brief Pointer to the plug model.
-    private: physics::ModelPtr plugModel;
+    protected: physics::ModelPtr plugModel;
 
     /// \brief Name of the plug link
-    private: std::string plugLinkName;
+    protected: std::string plugLinkName;
 
     /// \brief Pointer to the plug link.
-    private: physics::LinkPtr plugLink;
+    protected: physics::LinkPtr plugLink;
+
+    /// \brief Substring in all gripper link names
+    protected: std::string gripperLinkSubstring;
 
-    /// \brief Pinter to the prismatic joint formed between the plug and the
+    /// \brief Pointer to the prismatic joint formed between the plug and the
     /// socket.
-    private: physics::JointPtr prismaticJoint;
+    protected: physics::JointPtr prismaticJoint;
 
     /// \brief Boolean that indicates weather the prismatic joint is created.
-    private: bool joined = false;
+    protected: bool joined = false;
 
     /// \brief Boolean that indicates weather the plug and socket are fixed.
-    private: bool locked = false;
+    protected: bool locked = false;
 
     /// \brief Pointer to the update event connection.
-    private: gazebo::event::ConnectionPtr updateConnection;
+    protected: gazebo::event::ConnectionPtr updateConnection;
+
+    /// \brief Force applied to the plug link by the sensor plate or gripper
+    protected: ignition::math::Vector3d plugLinkForce;
 
     /// \brief Vector that contains forces (For the purpose of averaging).
-    private: std::vector<double> forcesBuffer;
+    protected: std::vector<double> forcesBuffer;
 
     /// \brief Time stamps associated with forces in the above vector.
-    private: std::vector<common::Time> timeStamps;
+    protected: std::vector<common::Time> timeStamps;
 
     /// \brief Time the plug and socket has been in alignment.
-    private: common::Time alignmentTime = 0;
+    protected: common::Time alignmentTime = 0;
 
     /// \brief Time the plug and the socket has been freed.
     /// Used to put some buffer between unfreezing and another possible mating.
-    private: common::Time unfreezeTimeBuffer = 0;
+    protected: common::Time unfreezeTimeBuffer = 0;
 
     /// \brief Roll alignment tolerance.
-    private: double rollAlignmentTolerance;
+    protected: double rollAlignmentTolerance;
 
     /// \brief pitch alignment tolerance.
-    private: double pitchAlignmentTolerance;
+    protected: double pitchAlignmentTolerance;
 
     /// \brief Yaw alignment tolerance.
-    private: double yawAlignmentTolerance;
+    protected: double yawAlignmentTolerance;
 
     /// \brief force required to mate the plug & socket (lock joint).
-    private: double matingForce;
+    protected: double matingForce;
 
     /// \brief force required to unmate the plug & socket (unlock joint).
-    private: double unmatingForce;
+    protected: double unmatingForce;
 
     // Some private counter variables ISO "throttled" logging/messages
 
@@ -125,18 +134,18 @@ namespace gazebo
     /// \brief Concatenates/trims forcesBuffer and timeStamps vectors to
     /// include only the last trimDuration.
     /// \param[in] trimDuration Duration over which to trim the vectors.
-    void trimForceVector(double trimDuration);
+    protected: void trimForceVector(double trimDuration);
 
     /// \brief Calculates the average of the forcesBuffer vector.
     /// \return Average of the forcesBuffer vector.
-    double movingTimedAverage();
+    protected: double movingTimedAverage();
 
     /// \brief Appends another force reading to the forcesBuffer vector.
-    void addForce(double force);
+    protected: void addForce(double force);
 
     /// \brief Utility function to normalize error angles to (-pi, pi]
     /// \return normalized angle
-    private: double normalizeError(double error);
+    protected: double normalizeError(double error);
 
     /// \brief Constructor.
     public: PlugAndSocketMatingPlugin();
@@ -152,40 +161,56 @@ namespace gazebo
 
     /// \brief Check if plug and socket have the same orientation and altitude.
     /// \return Return true if same r,p,y and z.
-    private: bool isAligned();
+    protected: bool isAligned();
 
     /// \brief Measure Euclidean distance between plug an socket.
     /// \return Return true if plug is close to the socket.
-    private: bool checkProximity();
+    protected: bool checkProximity();
 
     /// \brief Creates the prismatic joint between the socket and plug.
-    private: void constructJoint();
+    protected: void constructJoint();
 
     /// \brief Distroys the prismatic joint between the socket and the plug.
-    private: void removeJoint();
+    protected: void removeJoint();
 
     /// \brief Calculates the average force exerted by contact2 on contact 1.
     /// \return Average force exerted by contact2 on contact1.
-    public: bool averageForceOnLink(std::string contact1, std::string contact2);
+    protected: bool averageForceOnLink(std::string contact1,
+                                       std::string contact2);
 
     /// \brief Determine if Electrical Plug is pushing against electrical
     /// socket.
     /// \return boolean indicating whether the plug is pushing against the
     /// socket.
-    public: bool isPlugPushingSensorPlate(int numberOfDatapointsThresh = 10);
+    protected: bool isPlugPushingSensorPlate(int numberOfDatapointsThresh = 10);
 
     /// \brief Determine if Electrical Plug is pushing against electrical
     /// socket.
     /// \return boolean indicating whether the plug is pushing against the
     /// socket.
-    public: bool isEndEffectorPushingPlug(int numberOfDatapointsThresh = 10);
+    protected: bool isEndEffectorPushingPlug(int numberOfDatapointsThresh = 10);
 
     /// \brief Gets the collision index between two links.
     /// \return Collision index between contact1 and contact2.
-    public: int getCollisionBetween(std::string contact1, std::string contact2);
+    protected: std::vector<int> getCollisionsBetween(std::string contact1,
+                                                     std::string contact2);
 
     /// \brief Callback executed at every physics update.
     public: void Update();
+
+    // Some ROS stuff to support exposure of the forces on the link
+
+    /// \brief Namespace for ROS topics and services
+    protected: std::string ns;
+
+    /// \brief Pointer to this ROS node's handle.
+    protected: boost::shared_ptr<ros::NodeHandle> rosNode;
+
+    /// \brief Plug link applied force topic
+    protected: std::string linkForceTopic;
+
+    /// \brief Publisher for the plug link applied force in the link frame
+    protected: ros::Publisher linkForcePub;
   };
   GZ_REGISTER_MODEL_PLUGIN(PlugAndSocketMatingPlugin)
 }