Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update external wrench plugin with mutiple wrenches and improve rpc responses #418

Merged
merged 12 commits into from
Apr 16, 2019
Merged

Update external wrench plugin with mutiple wrenches and improve rpc responses #418

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
fb4b0e2
Handle unscoped link name
yeshasvitirupachuri Mar 27, 2019
36e2801
Add multiple wrenches functionality
yeshasvitirupachuri Mar 28, 2019
22c7f0c
Add operation mode support
yeshasvitirupachuri Mar 28, 2019
f796908
Update PR changes and rpc reply handling
yeshasvitirupachuri Mar 30, 2019
7366f2f
Add externalwrench readme
yeshasvitirupachuri Apr 9, 2019
9b70f79
Upate small PR change requests
yeshasvitirupachuri Apr 9, 2019
74f8c32
Update color rand generation
yeshasvitirupachuri Apr 9, 2019
2f5dcac
Implement onReset method
yeshasvitirupachuri Apr 9, 2019
64199f6
Update time handling
yeshasvitirupachuri Apr 9, 2019
ada8291
Update wrench count and index handling
yeshasvitirupachuri Apr 10, 2019
b7d1d38
Remove shared pointers and update to simple data structures
yeshasvitirupachuri Apr 10, 2019
c4da8fb
Update color handling using wrench index as seed
yeshasvitirupachuri Apr 15, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions plugins/externalwrench/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@ add_gazebo_yarp_plugin_target(LIBRARY_NAME externalwrench
INCLUDE_DIRS include/gazebo
SYSTEM_INCLUDE_DIRS ${Boost_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS} ${SDFORMAT_INCLUDE_DIRS} ${PROTOBUF_INCLUDE_DIRS}
LINKED_LIBRARIES ${YARP_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES}
HEADERS include/gazebo/ApplyExternalWrench.hh
SOURCES src/ApplyExternalWrench.cc
HEADERS include/gazebo/ApplyExternalWrench.hh include/gazebo/ExternalWrench.hh
SOURCES src/ApplyExternalWrench.cc src/ExternalWrench.cc
)


42 changes: 16 additions & 26 deletions plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,15 +8,17 @@
#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/LogStream.h>

#include "ExternalWrench.hh"

// A YARP Thread class that will be used to read the rpc port to apply external wrench
class RPCServerThread: public yarp::os::Thread
Expand All @@ -27,22 +29,26 @@ public:
virtual void threadRelease();
yarp::os::Bottle getCmd();
void setRobotName(std::string robotName);
void setScopedName(std::string scopedName);
void setDefaultLink(const std::string& defaultLink);
void setDurationBuffer(double d);
double getDurationBuffer();
void setRobotModel(physics::ModelPtr robotModel);
void setNewCommandFlag(std::int32_t flag);
virtual void onStop();

boost::shared_ptr< std::vector< boost::shared_ptr<ExternalWrench>>> wrenchesVectorPtr{new std::vector< boost::shared_ptr<ExternalWrench>>()};
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why can't we use a good old std::vector<ExternalWrench>?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Similar to #418 (comment)


private:
yarp::os::RpcServer m_rpcPort;
yarp::os::Bottle m_cmd;
yarp::os::Bottle m_reply;
/// \brief Mutex to lock reading and writing of _cmd
boost::mutex m_lock;
physics::ModelPtr m_robotModel;
std::string m_robotName;
std::string m_scopedName;
std::string m_defaultLink;
double m_durationBuffer;

// variable for operation mode
// single wrench or multiple wrenches
std::string m_mode;
std::string m_message;
};


Expand All @@ -56,41 +62,26 @@ private:
public:
ApplyExternalWrench();
virtual ~ApplyExternalWrench();
std::string retrieveSubscope(gazebo::physics::Link_V& v, std::string scope);
void applyWrenches();

struct wrench {
yarp::sig::Vector force;
yarp::sig::Vector torque;
double duration;
};
bool getCandidateLink(physics::LinkPtr& candidateLink, std::string candidateLinkName);

/// \brief Robot name that will be used to open rpc port
std::string robotName;
double timeIni;
// yarp::os::Bottle bufferBottle;


protected:
// Inherited
void Load ( physics::ModelPtr _model, sdf::ElementPtr _sdf );
// Inherited
virtual void UpdateChild();


private:
yarp::os::Network m_yarpNet;
RPCServerThread m_rpcThread;
yarp::os::Property m_iniParams;

physics::ModelPtr m_myModel;
/// \brief Link on which the wrench will be applied
std::string m_modelScope;
std::string m_subscope;
std::string m_linkName;

lrapetti marked this conversation as resolved.
Show resolved Hide resolved
/// \brief Link the plugin is attached to
physics::LinkPtr m_onLink;
/// \brief Wrench to be applied on the body
wrench m_wrenchToApply;

/// \brief Mutex to lock access
boost::mutex m_lock;
Expand All @@ -102,7 +93,6 @@ private:
msgs::Visual m_visualMsg;

bool m_newCommand;

};

}
Expand Down
83 changes: 83 additions & 0 deletions plugins/externalwrench/include/gazebo/ExternalWrench.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
#ifndef EXTERNALWRENCH_H
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use a less ambiguous header guard (something like GAZEBO_YARP_PLUGINS_EXTERNALWRENCH_H) to avoid possible collisions.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Furthermore, please add a copyright notice, something like:

/*
 * Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT)
 * All rights reserved.
 *
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 */

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

#define 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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Everything seems to be ok from what I understand, but static variables are always a bit tricky do deal with.
I don't think this needs to block the PR, but I think you can obtain something similar by setting the count to the in ExternalWrench constructor as you build it in the plugin class.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@traversaro I am not following you exactly. The count variable is actually increased in the constructor of ExternalWrench

gazebo-yarp-plugins/plugins/externalwrench/src/ExternalWrench.cc

Line 22 in f796908

count++;

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, but what happens if there are two different externalwrench plugins in the same model? As far as I was able to see, everything should work fine, but keeping count as a static variable may lead to some subtle bugs if this code is modified in the future.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

what happens if there are two different externalwrench plugins in the same model?

At the moment this is not possible to do by a user as there will be port address conflict while trying to open the rpc port with the model name. So, I believe the usage of count as static is good in this case.

Copy link
Member

@traversaro traversaro Apr 9, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

And what about two different externalwrench plugins in two different model that are spawned together? The count would be shared also in this case.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

While writing https://github.com/robotology/gazebo-yarp-plugins/pull/418/files#r273692453 , I am a bit confused by the count logic even in the case of the single plugin. Let's say that three wrenches are added, and they get the count 1, 2 and 3 , where 1 and 3 have a duration of 100 seconds, and 2 instead has a duration of 1 seconds. After the the wrench 2 is deleted, the count counter is decremented to 2. If (let's say after 10 seconds) a new wrench is added, the count counter is increased again, and so the new wrench gets a count of 3, that is a index that is already used by the wrench 3, and the visual representation will probably conflict!

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The conflict will happen for the visuals. So, it is better to not decrease the count of the wrenches after the duration is done. It is better to keep an increasing wrenchCount variable in ApplyExternalWrench and set the wrenchIndex variable of ExternalWrench class. This way each new wrench will have a different color without conflicts.

float color[4];
struct wrenchCommand
{
std::string link_name;
yarp::sig::Vector force;
yarp::sig::Vector torque;
double duration;
};

std::unique_ptr<wrenchCommand> wrenchPtr{new wrenchCommand()};
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am a bit confused. Why are you using std::unique_ptr here?
Why not a simple attribute of the class:

  wrenchCommand wrench;

or even better, given that the structure wrenchCommand is not used elsewhere, an anonymous struct:

   struct
   {
        std::string              link_name;
        yarp::sig::Vector        force;
        yarp::sig::Vector        torque;
        double                   duration;
   } wrench;

or given that the structure is never used standalone, why not simply including the attributes of the structure as attributes of the class?

Copy link
Member Author

@yeshasvitirupachuri yeshasvitirupachuri Apr 9, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@traversaro I remember back in time starting out with simple data structures without pointers but then I quickly ran into some problems while handling dynamic wrench addition and removal in multiple wrench operation mode. That is why I belive I migrated to using pointer to handle them correctly. Now, I tested to change back to simple data structures again and in the multiple wrench case the visuals are funny looking

ezgif com-video-to-gif

But with the pointers usage it is stable

ezgif com-video-to-gif (1)

At the moment I prefer not to investigate it further and I would like to stick to the pointers usage.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You obtain this behavior just by changing std::unique_ptr<wrenchCommand> wrenchPtr{new wrenchCommand()}; to wrenchCommand wrench;? If that is the case, it is extremely strange and I do not know what's going on.

If instead you obtain this because you changed boost::shared_ptr<ExternalWrench> newWrench(new ExternalWrench); to ExternalWrench newWrench and other changes related to ExternalWrench, I guess what is going on is that there is something going on related to the use of static count, that is increased whenever a new copy of ExternalWrench is created, but not when a new copy of the shared_ptr is created. This is exactly the tricky behavior related to static variables (that are actually just global variables a bit disguised) that I was mentioning in https://github.com/robotology/gazebo-yarp-plugins/pull/418/files#r273442326 .
I think the plugin will be much more maintainable if you just drop the static count and just have a normal local index or count attribute that can be set with a SetIndex or SetCount method, that can contain all the code currently contained in the constructor. This SetIndex/SetCount method can be set directly in the https://github.com/robotology/gazebo-yarp-plugins/pull/418/files#diff-6f34d47dcd5558cbef158050b5563c76R175 , using some kind of variable shared between RPCServerThread and ApplyExternalWrench that is increase/decreased appropriately.

Copy link
Member Author

@yeshasvitirupachuri yeshasvitirupachuri Apr 10, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This behavior is after changing std::unique_ptr<wrenchCommand> wrenchPtr{new wrenchCommand()}; to wrenchCommand wrench; and also boost::shared_ptr< std::vector< boost::shared_ptr<ExternalWrench>>> wrenchesVectorPtr{new std::vector< boost::shared_ptr<ExternalWrench>>()}; to std::vector<ExternalWrench> wrenchesVector. Anyway, I do agree that static count variable will eventually cause problems when having multiple models having the plugin. So, I will have a variable wrenchesIndex in ApplyExternalWrench class and set the wrenchIndex variable in ExternalWrench class.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

static count is removed and updated with new wrench count and index variables ada8291

Shared points are removed and simple data structures are used b7d1d38


double tick;
double tock;

bool model_has_link;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This variables does not seem to be ever read, but just write too. Can it be removed?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

physics::ModelPtr model;
physics::LinkPtr link;
physics::Link_V links;

transport::NodePtr m_node;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you are using the m_ prefix for attribute variables, I suggest to do it consistently for all the attributes of the class.

Copy link
Member Author

@yeshasvitirupachuri yeshasvitirupachuri Apr 9, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done, removed m_ prefix

transport::PublisherPtr m_visPub;
msgs::Visual m_visualMsg;

event::ConnectionPtr updateConnection;

void setVisual();

public:

bool duration_done;

ExternalWrench();
~ExternalWrench();

bool setWrench(physics::ModelPtr&, yarp::os::Bottle&);
bool getLink();

void applyWrench();
void deleteWrench();
void setModel();
};

#endif
Loading