Multiple External Wrenches in Gazebo

plugins/CMakeLists.txt

@@ -2,6 +2,7 @@ add_subdirectory(camera)
 add_subdirectory(clock)
 add_subdirectory(controlboard)
 add_subdirectory(externalwrench)
+add_subdirectory(multiexternalwrench)
 add_subdirectory(forcetorque)
 add_subdirectory(imu)
 add_subdirectory(jointsensors)

plugins/multiexternalwrench/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8.7)
+
+PROJECT(Plugin_MultiExternalWrench)
+
+include(AddGazeboYarpPluginTarget)
+
+set(multiexternalwrench_headers include/externalwrench.h include/applymultiexternalwrench.h)
+set(multiexternalwrench_sources src/externalwrench.cpp src/applymultiexternalwrench.cpp)
+
+set(LIB_COMMON_NAME gazebo_yarp_lib_common)
+if(CMAKE_VERSION VERSION_LESS 3.0.0)
+    get_property(GAZEBO_YARP_COMMON_HEADERS GLOBAL PROPERTY GAZEBO_YARP_COMMON_HEADERS)
+    unset(LIB_COMMON_NAME)
+endif()
+
+add_gazebo_yarp_plugin_target(LIBRARY_NAME multiexternalwrench
+                              INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}
+                              INCLUDE_DIRS include/gazebo
+                              SYSTEM_INCLUDE_DIRS ${GAZEBO_YARP_COMMON_HEADERS} ${YARP_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS}  ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS}
+                              LINKED_LIBRARIES ${LIB_COMMON_NAME} gazebo_yarp_singleton ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}
+                              HEADERS ${multiexternalwrench_headers}
+                              SOURCES ${multiexternalwrench_sources}
+                                 )
+
+

plugins/multiexternalwrench/include/applymultiexternalwrench.h

@@ -0,0 +1,61 @@
+#ifndef APPLYMULTIEXTERNALWRENCH_H
+#define APPLYMULTIEXTERNALWRENCH_H
+
+#include <externalwrench.h>
+#include <boost/shared_ptr.hpp>
+
+class RPCServerThread: public yarp::os::Thread
+{
+private:
+
+    //boost::shared_ptr<ExternalWrench> newWrench;
+
+    yarp::os::RpcServer m_rpcPort;
+    yarp::os::Bottle m_cmd;
+    yarp::os::Bottle m_reply;
+
+    std::string m_robotName;
+    physics::ModelPtr m_robotModel;
+
+public:
+    boost::mutex m_lock;
+
+    boost::shared_ptr< std::vector< boost::shared_ptr<ExternalWrench> > > wrenchesVectorPtr{new std::vector< boost::shared_ptr<ExternalWrench>>()};
+
+
+    virtual bool        threadInit();
+    virtual void        run();
+    virtual void        threadRelease();
+    void                setRobotName(std::string robotName);
+    void setRobotModel(physics::ModelPtr robotModel);
+
+    yarp::os::Bottle    getCmd();
+};
+
+namespace gazebo
+{
+    class ApplyMultiExternalWrench: public ModelPlugin
+    {
+    public:
+        ApplyMultiExternalWrench();
+        ~ApplyMultiExternalWrench();
+        void applyWrenchs();
+        void Load (physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+    private:
+        yarp::os::Network m_yarpNet;
+        RPCServerThread m_rpcThread;
+        yarp::os::Property m_iniParams;
+
+        physics::ModelPtr m_myModel;
+        std::string robotName;
+
+        boost::mutex m_lock;
+        event::ConnectionPtr m_updateConnection;
+
+    };
+}
+
+
+
+#endif

plugins/multiexternalwrench/include/externalwrench.h

@@ -0,0 +1,84 @@
+#ifndef YARPGAZEBO_EXTERNALWRENCH_H
+#define YARPGAZEBO_EXTERNALWRENCH_H
+
+
+#include <iostream>
+#include <stdlib.h>
+#include <ctime>
+#include <stdio.h>
+#include <string>
+#include <memory>
+
+#include "gazebo/gazebo.hh"
+#include <gazebo/physics/PhysicsEngine.hh>
+#include <gazebo/common/Events.hh>
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/transport/Node.hh>
+#include <gazebo/physics/Link.hh>
+#include <gazebo/physics/World.hh>
+#include <gazebo/physics/Model.hh>
+#include <GazeboYarpPlugins/common.h>
+
+#include <yarp/os/Network.h>
+#include <yarp/os/RpcServer.h>
+#include <yarp/os/Bottle.h>
+#include <yarp/sig/Vector.h>
+#include <yarp/os/Thread.h>
+#include <yarp/os/Time.h>
+#include <yarp/os/Vocab.h>
+#include <yarp/os/Log.h>
+#include <yarp/os/LogStream.h>
+#include <yarp/math/Math.h>
+
+#include <boost/lexical_cast.hpp>
+
+using namespace gazebo; 
+
+class ExternalWrench
+{
+private:
+
+   static int count;
+   float color[4];
+   struct wrenchCommand
+   {
+        std::string link_name;
+        gazebo::math::Vector3 force;
+        gazebo::math::Vector3 torque;
+        double duration;
+   };
+
+   std::unique_ptr<wrenchCommand> wrenchPtr{new wrenchCommand()};
+
+   double tick;
+   double tock;
+   gazebo::math::Vector3 *force_;
+   gazebo::math::Vector3 *torque_;
+
+   bool model_has_link;
+   physics::ModelPtr model;
+   physics::LinkPtr link;
+   physics::Link_V model_links;
+
+   transport::NodePtr m_node;
+   transport::PublisherPtr m_visPub;
+   msgs::Visual m_visualMsg;
+
+   event::ConnectionPtr updateConnection;
+
+
+public:
+
+    bool duration_done;
+
+    ExternalWrench();
+    ~ExternalWrench();
+
+    bool setWrench(physics::ModelPtr&, yarp::os::Bottle&);
+    bool getLink();
+    void applyWrench();
+    void setModel();
+};
+
+
+#endif

plugins/multiexternalwrench/src/applymultiexternalwrench.cpp

@@ -0,0 +1,173 @@
+#include <applymultiexternalwrench.h>
+
+namespace gazebo
+{
+    GZ_REGISTER_MODEL_PLUGIN(ApplyMultiExternalWrench)
+
+ApplyMultiExternalWrench::ApplyMultiExternalWrench()
+{
+}
+
+ApplyMultiExternalWrench::~ApplyMultiExternalWrench()
+{
+    /*m_rpcThread.m_lock.lock();
+    for(int i = 0; i < m_rpcThread.wrenchesVectorPtr->size() ; i++)
+    {
+        if(m_rpcThread.wrenchesVectorPtr->at(i)->duration_done)
+        {
+             delete m_rpcThread.wrenchesVectorPtr->at(i);
+        }
+
+    }
+    m_rpcThread.m_lock.unlock();*/
+    m_rpcThread.stop();
+    this->m_updateConnection.reset();
+}
+
+void ApplyMultiExternalWrench::Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf )
+{
+    if ( !this->m_yarpNet.checkNetwork() ) {
+        yError ( "ERROR Yarp Network was not found active in ApplyExternalWrench plugin" );
+        return;
+    }
+
+    // Copy the pointer to the model to access later from UpdateChild
+    this->m_myModel = _model;
+
+    bool configuration_loaded = false;
+
+    // Read robot name
+    if ( _sdf->HasElement ( "robotNamefromConfigFile" ) ) {
+        std::string iniRobotName      = _sdf->Get<std::string> ( "robotNamefromConfigFile" );
+        std::string iniRobotNamePath =  gazebo::common::SystemPaths::Instance()->FindFileURI ( iniRobotName );
+
+        if ( iniRobotNamePath != "" && this->m_iniParams.fromConfigFile ( iniRobotNamePath.c_str() ) ) {
+            yarp::os::Value robotNameParam = m_iniParams.find ( "gazeboYarpPluginsRobotName" );
+            this->robotName = robotNameParam.asString();
+
+            m_rpcThread.setRobotName  ( robotName );
+            m_rpcThread.setRobotModel(_model);
+            configuration_loaded = true;
+        } else {
+            yError ( "ERROR trying to get robot configuration file" );
+            return;
+        }
+    } else {
+            this->robotName = _model->GetName();
+            m_rpcThread.setRobotName ( robotName );
+            configuration_loaded = true;
+    }
+
+    // Starting RPC thread to read desired wrench to be applied
+    if ( !m_rpcThread.start() ) {
+        yError ( "ERROR: rpcThread did not start correctly" );
+    }
+
+    this->m_updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ApplyMultiExternalWrench::applyWrenchs,this));
+}
+
+}
+
+void ApplyMultiExternalWrench::applyWrenchs()
+{
+    //Now check for duration done flag from all the wrench threads and removes the ones that are done
+    //yInfo() << "Applying external wrenches";
+    //yInfo() << "Number of external wrenches : " << m_rpcThread.wrenchThreads.size();
+    m_rpcThread.m_lock.lock();
+    for(int i = 0; i < m_rpcThread.wrenchesVectorPtr->size() ; i++)
+    {
+        bool duration_check = m_rpcThread.wrenchesVectorPtr->at(i)->duration_done;
+        if(duration_check==false)
+        {
+            m_rpcThread.wrenchesVectorPtr->at(i)->applyWrench();
+        }
+        else
+        {
+            //yInfo() << "External wrench duration done";
+        }
+    }
+    m_rpcThread.m_lock.unlock();
+}
+
+void RPCServerThread::setRobotModel(physics::ModelPtr robotModel)
+{
+    m_robotModel = robotModel;
+}
+
+
+bool RPCServerThread::threadInit()
+{
+
+    if ( !m_rpcPort.open ( std::string ( "/"+m_robotName + "/applyMultiExternalWrench/rpc:i" ).c_str() ) ) {
+        yError ( "ERROR opening RPC port /applyExternalWrench" );
+        return false;
+    }
+
+    return true;
+}
+
+void RPCServerThread::run()
+{
+    while(!isStopping())
+    {
+        yarp::os::Bottle command;
+        m_rpcPort.read ( command,true );
+        if(command.get(0).asString() == "help")
+        {
+            this->m_reply.addVocab ( yarp::os::Vocab::encode ( "many" ) );
+            this->m_reply.addString ( "Insert a command with the following format:" );
+            this->m_reply.addString ( "[link] [force] [torque] [duration]" );
+            this->m_reply.addString ( "e.g. chest 10 0 0 0 0 0 1");
+            this->m_reply.addString ( "[link]:     (string) Link ID of the robot as specified in robot's SDF" );
+            this->m_reply.addString ( "[force]:    (double x, y, z) Force components in N w.r.t. world reference frame" );
+            this->m_reply.addString ( "[torque]:   (double x, y, z) Torque components in N.m w.r.t world reference frame" );
+            this->m_reply.addString ( "[duration]: (double) Duration of the applied force in seconds" );
+            this->m_reply.addString ( "Note: The reference frame is the base/root robot frame with x pointing backwards and z upwards.");
+            this->m_rpcPort.reply ( this->m_reply );
+        }
+        else{
+            if(command.get(0).isString() \
+                    && ( command.get ( 1 ).isDouble() || command.get ( 1 ).isInt() )  && ( command.get ( 2 ).isDouble() || command.get ( 2 ).isInt() ) && ( command.get ( 3 ).isDouble() || command.get ( 3 ).isInt() ) \
+                    && ( command.get ( 4 ).isDouble() || command.get ( 4 ).isInt() )  && ( command.get ( 5 ).isDouble() || command.get ( 5 ).isInt() ) && ( command.get ( 6 ).isDouble() || command.get ( 6 ).isInt() )  \
+                    && ( command.get ( 7 ).isDouble() || command.get ( 7 ).isInt() ) ) {
+                this->m_reply.addString ( "[ACK] Correct command format" );
+                this->m_rpcPort.reply ( m_reply );
+                m_lock.lock();
+                // new-command flag
+                command.addInt(1);
+                m_cmd = command;
+                m_lock.unlock();
+
+               //Creating new instances of external wrenches
+               boost::shared_ptr<ExternalWrench> newWrench(new ExternalWrench);
+               if(newWrench->setWrench(m_robotModel,m_cmd))
+               {
+                   wrenchesVectorPtr->push_back(newWrench);            
+               }
+               else yError() << "Failed to set new wrench values!";
+            } else {
+                this->m_reply.clear();
+                this->m_reply.addString ( "ERROR: Incorrect command format" );
+                this->m_rpcPort.reply ( this->m_reply );
+            }
+        }
+        m_reply.clear();
+        command.clear();   
+    }
+}
+
+void RPCServerThread::setRobotName ( std::string robotName )
+{
+    this->m_robotName = robotName;
+}
+
+void RPCServerThread::threadRelease()
+{
+    yarp::os::Thread::threadRelease();
+    m_rpcPort.close();
+}
+
+yarp::os::Bottle RPCServerThread::getCmd()
+{
+    return m_cmd;
+}