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

yeshasvitirupachuri · 2019-03-28T18:41:03Z

Following the limitations of the externalWrench documented in #417 this PR updated the plugin with new functionalities.

traversaro · 2019-03-28T19:36:11Z

Can we find a reviewer in the lab? (Perhaps @lrapetti?) I am a bit short of time till next week.

plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh

plugins/externalwrench/src/ApplyExternalWrench.cc

plugins/externalwrench/src/ExternalWrench.cc

lrapetti · 2019-04-01T11:39:33Z

Looks Good To Me,
@traversaro I think we can merge.

lrapetti · 2019-04-03T09:40:56Z

It would be better to also create a README.md for the externawrench plugin (for example like the one that can be found for the linkattacher).

traversaro · 2019-04-03T20:24:52Z

I have added a few comments to the PR as well. For future PR, can you use human understandable PR titles, in place of the one automatically generated by github from the branch names? Thanks!

traversaro

Some comments.
Bonus points: what happens if the simulation is reset while a force is being applied? See

gazebo-yarp-plugins/plugins/controlboard/src/ControlBoardDriver.cpp

Line 341 in 01ad1d1

this->m_resetConnection =

for an example on how to deal with the reset event.

traversaro · 2019-04-03T20:21:56Z

plugins/externalwrench/include/gazebo/ExternalWrench.hh

@@ -0,0 +1,83 @@
+#ifndef EXTERNALWRENCH_H


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

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 */

traversaro · 2019-04-03T20:28:16Z

plugins/externalwrench/src/ExternalWrench.cc

+    m_visPub = this->m_node->Advertise<msgs::Visual>("~/visual",100);
+
+    // Set the visual's name. This should be unique.
+    std::string visual_name = "__" + wrenchPtr->link_name + "__CYLINDER_VISUAL__" + boost::lexical_cast<std::string>(count);


I think we can avoid the unnecessary dependency on boost by just using C++11's std::to_string .
If the identifier needs to be unique, I suggest to add a prefix unique to this plugin, such as GYP_EXT_WRENCH_ , that can be useful if somebody needs to understand the nature of a given visual.

traversaro · 2019-04-03T20:31:09Z

plugins/externalwrench/src/ApplyExternalWrench.cc

                m_lock.lock();
                // new-command flag
-                command.addInt32(1);
+                command.addInt(1);


If I remember correctly, addInt32 is the suggested method to use. Why revert back to addInt ?

not the suggested method to use

not the suggested method or is the suggested method?

Sorry, is the suggested method, I fixed the comment.

traversaro · 2019-04-03T20:32:14Z

plugins/externalwrench/src/ExternalWrench.cc

+{
+    model_has_link = false;
+
+    //srand(boost::lexical_cast<float>(std::time(NULL)));


Can we remove this?

traversaro · 2019-04-03T20:41:48Z

plugins/externalwrench/include/gazebo/ExternalWrench.hh

+        double                   duration;
+   };
+
+   std::unique_ptr<wrenchCommand> wrenchPtr{new wrenchCommand()};


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?

@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

But with the pointers usage it is stable

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

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.

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.

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

traversaro · 2019-04-03T20:48:44Z

plugins/externalwrench/include/gazebo/ExternalWrench.hh

+   double                        tick;
+   double                        tock;
+
+   bool                          model_has_link;


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

traversaro · 2019-04-03T20:57:44Z

plugins/externalwrench/src/ExternalWrench.cc

+    model_has_link = false;
+
+    //srand(boost::lexical_cast<float>(std::time(NULL)));
+    srand(rand()%100);


C random functions should never be used in modern C++, as they implicitly modify the global state of the program, that as you may imagine is not ideal. Furthermore, what is the rationale of seeding the random generator with the last value of the random generator divided by 100?

In general, especially in a simulation program, I would never use randomness unless the seed of the used random generator is exposed to the user, otherwise you make the simulation output (that in a sense includes also the visualization!) non reproducible. You may think this is not a huge problem, but imagine that you have a series of screen for a paper, and you need to reproduce exactly it again but in an higher resolution for some reason: this would be impossible with this implementation.

To keep it simple, I think the best option is just to assign the color based on the count variable: either you initialize a local random generator that you seed with count for getting the color components, or you simply keep a table of a few colors (for example 20) and you assign colors using colors[count%20].

either you initialize a local random generator that you seed with count for getting the color components

@traversaro what you mean by "local random generator" ?

Something like:

std::mt19937 gen(42); std::uniform_real_distribution<> dis(0.0, 1.0); for(int i=0; i < 10; i++) { std::cerr << "Random " << i << " : " << dis(gen) << std::endl; }

Given that you seed it with a fixed number (42) the output of this function it is deterministic.

Updated color random generation a4323de

traversaro · 2019-04-03T21:03:25Z

plugins/externalwrench/src/ExternalWrench.cc

+    wrenchPtr->duration = 0.0;
+
+    count++;
+    tick = yarp::os::Time::now();


I am afraid that in general this will not give you the expected duration of the external force, unless gzserver is executed with the YARP_CLOCK=/clock env variable and with the gazebo_yarp_clock plugin. If one of this conditions is not true, yarp::os::Time::now() will return you the system time, resulting in a duration of the external force in simulation that may be much shorter than expected (depending on the RTF) and even worse a duration that is not reproducible.

The proper solution to this is to add a const gazebo::common::UpdateInfo& _info argument to the applyWrenches method, and use it to retrieved the simulation time step. See other onUpdate methods in the repo to get an inspiration https://github.com/robotology/gazebo-yarp-plugins/search?q=onUpdate&unscoped_q=onUpdate .

Time handling is updated 64199f6

traversaro · 2019-04-03T21:10:47Z

plugins/externalwrench/include/gazebo/ApplyExternalWrench.hh

    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>>()};


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

Similar to #418 (comment)

traversaro · 2019-04-03T21:17:26Z

plugins/externalwrench/include/gazebo/ExternalWrench.hh

+{
+private:
+
+   static int count;


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.

@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++;

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.

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.

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

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!

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.

yeshasvitirupachuri · 2019-04-09T09:25:30Z

It would be better to also create a README.md for the externawrench plugin (for example like the one that can be found for the linkattacher).

README file added 7366f2f

yeshasvitirupachuri · 2019-04-09T10:19:34Z

Some comments.
Bonus points: what happens if the simulation is reset while a force is being applied? See

gazebo-yarp-plugins/plugins/controlboard/src/ControlBoardDriver.cpp

Line 341 in 01ad1d1

this->m_resetConnection =
for an example on how to deal with the reset event.

On resetting gazebo, the wrenches are still applied as provided by the user commands. Do you think the wrenches should be cleared? @traversaro

traversaro · 2019-04-09T10:46:33Z

On resetting gazebo, the wrenches are still applied as provided by the user commands. Do you think the wrenches should be cleared? @traversaro

My understanding of reset is that the simulation should restart from the initial condition. If a user asked for an external force of 1 N on one link for one hour, I think after a reset, the force should be cleared. Think for example of an RL scenario: would it make sense to keep the old force?

yeshasvitirupachuri · 2019-04-09T11:10:38Z

On resetting gazebo, the wrenches are still applied as provided by the user commands. Do you think the wrenches should be cleared? @traversaro

My understanding of reset is that the simulation should restart from the initial condition. If a user asked for an external force of 1 N on one link for one hour, I think after a reset, the force should be cleared. Think for example of an RL scenario: would it make sense to keep the old force?

Agreed!
Added onReset() method 2ddd481. On gazebo world reset, all the existing wrenches are cleared and the external wrench plugin is set to default single wrench operation mode.

Update visual name handling

Update readme with gazebo reset behavior

yeshasvitirupachuri · 2019-04-14T17:40:09Z

@traversaro any update on this ?

traversaro · 2019-04-14T18:11:43Z

@traversaro any update on this ?

Sorry @Yeshasvitvs , I do not get notifications for new commits pushed in the PR, and even if I got them I have no way to know if all the review comments has been addressed. From your comment I imagine that the PR is ready for a new review round, but for the future feel free to comment on the PR when the review comments has been addressed, so the reviewer can check the new code.

yeshasvitirupachuri · 2019-04-14T22:05:34Z

All the previous reviews comments are address in the recent commits 😉 please check them and let me know if there are more changes needed. Thanks @traversaro

traversaro · 2019-04-15T07:37:52Z

There are some comments below, but nothing blocking the merge. Do you want to cleanup the commits or otherwise I will squash the commits as part of the merge.

Now the ExternalWrench::setWrenchColor function will take more and more time as simulation goes on, am I right. This is probably not desirable, especially if the wrenchIndex will grow a lot (something like ~10000 or similar).

yeshasvitirupachuri · 2019-04-15T15:38:16Z

@traversaro Now the ExternalWrench::setWrenchColor function will take more and more time as simulation goes on

yes that is the way it is coded now

This is probably not desirable, especially if the wrenchIndex will grow a lot (something like ~10000 or similar)

Yes but it is an extreme case. Can you think of any other way to handle the uniform distribution through ApplyExternalWrench class ??

traversaro · 2019-04-15T15:43:48Z

Yes but it is an extreme case. Can you think of any other way to handle the uniform distribution through ApplyExternalWrench class ??

std::mt19937 rand_gen(wrench_index);
std::uniform_real_distribution<> uni_dis(0.0, 1.0)

yeshasvitirupachuri · 2019-04-15T17:03:50Z

Yes but it is an extreme case. Can you think of any other way to handle the uniform distribution through ApplyExternalWrench class ??
std::mt19937 rand_gen(wrench_index);
std::uniform_real_distribution<> uni_dis(0.0, 1.0)

@traversaro updated color handling using wrench index as seed c4da8fb

traversaro · 2019-04-16T05:12:36Z

Ok, do you want to cleanup the commits? Otherwise I will squash the commits as part of the merge.

yeshasvitirupachuri · 2019-04-16T07:31:45Z

@traversaro please take care of the commits while merging. Thanks

yeshasvitirupachuri added 3 commits March 28, 2019 00:17

Handle unscoped link name

fb4b0e2

Add multiple wrenches functionality

36e2801

Add operation mode support

22c7f0c

lrapetti self-requested a review March 29, 2019 07:43

lrapetti reviewed Mar 29, 2019

View reviewed changes

Update PR changes and rpc reply handling

f796908

lrapetti assigned lrapetti and unassigned lrapetti Apr 1, 2019

lrapetti approved these changes Apr 1, 2019

View reviewed changes

lrapetti assigned yeshasvitirupachuri Apr 1, 2019

yeshasvitirupachuri mentioned this pull request Apr 3, 2019

Limitations of externalWrench plugin #417

Closed

3 tasks

traversaro requested changes Apr 3, 2019

View reviewed changes

yeshasvitirupachuri changed the title ~~Feature/update external wrench~~ Update external wrench plugin with mutiple wrenches and improve rpc responses Apr 4, 2019

Add externalwrench readme

7366f2f

yeshasvitirupachuri added 6 commits April 9, 2019 15:26

Upate small PR change requests

9b70f79

Update color rand generation

74f8c32

Update visual name handling

Implement onReset method

2f5dcac

Update readme with gazebo reset behavior

Update time handling

64199f6

Update wrench count and index handling

ada8291

Remove shared pointers and update to simple data structures

b7d1d38

traversaro approved these changes Apr 15, 2019

View reviewed changes

Update color handling using wrench index as seed

c4da8fb

traversaro merged commit 37eff49 into robotology:devel Apr 16, 2019

This was referenced Apr 18, 2019

Linking on gazebo_yarp_externalWrench plugin is broken on devel #420

Closed

externalwrench and Gazebo step #413

Closed

traversaro mentioned this pull request Jun 3, 2019

Add release notes document #425

Closed

This was referenced Jan 28, 2020

Add in the externalwrench plugin the option to specify the orientation frame "local" #462

Open

Add the option for the external force orientation to be reperesented in the link frame #463

Closed

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

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

Conversation

yeshasvitirupachuri commented Mar 28, 2019

traversaro commented Mar 28, 2019

lrapetti commented Apr 1, 2019

lrapetti commented Apr 3, 2019

traversaro commented Apr 3, 2019

traversaro left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro Apr 3, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yeshasvitirupachuri Apr 9, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yeshasvitirupachuri Apr 10, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro Apr 9, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yeshasvitirupachuri commented Apr 9, 2019

yeshasvitirupachuri commented Apr 9, 2019 • edited

traversaro commented Apr 9, 2019

yeshasvitirupachuri commented Apr 9, 2019 • edited

yeshasvitirupachuri commented Apr 14, 2019

traversaro commented Apr 14, 2019

yeshasvitirupachuri commented Apr 14, 2019

traversaro commented Apr 15, 2019

yeshasvitirupachuri commented Apr 15, 2019

traversaro commented Apr 15, 2019

yeshasvitirupachuri commented Apr 15, 2019

traversaro commented Apr 16, 2019

yeshasvitirupachuri commented Apr 16, 2019

traversaro Apr 3, 2019 •

edited

yeshasvitirupachuri Apr 9, 2019 •

edited

yeshasvitirupachuri Apr 10, 2019 •

edited

traversaro Apr 9, 2019 •

edited

yeshasvitirupachuri commented Apr 9, 2019 •

edited

yeshasvitirupachuri commented Apr 9, 2019 •

edited