{"action":"created","issue":{"url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins/issues/392","repository_url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins","labels_url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins/issues/392/labels{/name}","comments_url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins/issues/392/comments","events_url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins/issues/392/events","html_url":"https://github.com/robotology/gazebo-yarp-plugins/issues/392","id":372919978,"node_id":"MDU6SXNzdWUzNzI5MTk5Nzg=","number":392,"title":"Plugin for setting initial joint position of included and nested models","user":{"login":"traversaro","id":1857049,"node_id":"MDQ6VXNlcjE4NTcwNDk=","avatar_url":"https://avatars2.githubusercontent.com/u/1857049?v=4","gravatar_id":"","url":"https://api.github.com/users/traversaro","html_url":"https://github.com/traversaro","followers_url":"https://api.github.com/users/traversaro/followers","following_url":"https://api.github.com/users/traversaro/following{/other_user}","gists_url":"https://api.github.com/users/traversaro/gists{/gist_id}","starred_url":"https://api.github.com/users/traversaro/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/traversaro/subscriptions","organizations_url":"https://api.github.com/users/traversaro/orgs","repos_url":"https://api.github.com/users/traversaro/repos","events_url":"https://api.github.com/users/traversaro/events{/privacy}","received_events_url":"https://api.github.com/users/traversaro/received_events","type":"User","site_admin":false},"labels":[],"state":"open","locked":false,"assignee":null,"assignees":[],"milestone":null,"comments":25,"created_at":"2018-10-23T10:12:05Z","updated_at":"2018-11-16T14:42:30Z","closed_at":null,"author_association":"MEMBER","body":"This is based on discussion that we had some time ago with @nunoguedelha . \r\n\r\n## Problem \r\nWe would like to specify initial joint positions for included models, something like:\r\n~~~xml\r\n<include>\r\n    <uri>model://icub</uri>\r\n    <pose>0.0 0 0.6 0 0 3.14</pose>\r\n    <!-- Specify joint position- -> \r\n     ... \r\n</include>\r\n~~~\r\n\r\n## Current situation \r\nThere is a Gazebo issue open on this, see https://bitbucket.org/osrf/gazebo/issues/1072/allow-to-set-initial-joint-pose-in-sdf , and the `gazebo_yarp_controlboard` plugin offers a similar feature using the `initialConfiguration` option https://github.com/robotology/gazebo-yarp-plugins/blob/01ad1d1252b0827033fd57058f066cc790b893c6/plugins/controlboard/src/ControlBoardDriver.cpp#L353 . However, there is no way at the moment to overwrite this parameter in an external model. \r\n\r\n## Possible solution \r\nI think it is possible to workaround this situation by exploiting the order in which the `Load` method of a [ModelPlugin](http://gazebosim.org/tutorials?tut=plugins_model). I think we can add a `set_initial_position` plugin, that would be loaded in the parent `model` **after** the inclusion of the model that contains the `gazebo-yarp-plugins`. This plugin could call the [`gazebo::physics::Joint::SetPosition`](http://osrf-distributions.s3.amazonaws.com/gazebo/api/dev/classgazebo_1_1physics_1_1Joint.html#ad02e021e1745f416f45e13024925714a) method in its `Load`, that should be called by after the `Load` of  any plugin of the model that has been included (this should be checked).\r\n In this way the the initial configuration of the robot should be the desired one: the only problem would be that any internal buffer of the plugin that handles joint control (in particular of `gazebo_yarp_controlboard`) should be properly changed as well. I think the best strategy for dealing with this is to change the `gazebo_yarp_controlboard` to get the initial joint positions for the its controller and its trajectory generated during the first call to `Update`, instead of during the Load. We need to check, but I hope the first call to update of any plugin in the model should be done **after** the `Load` methods for all the plugins as been completed. (See https://bitbucket.org/osrf/gazebo/src/04a40b56457055a5d373bf6d01a0cf72c3021416/gazebo/physics/World.cc#lines-1058 vs https://bitbucket.org/osrf/gazebo/src/04a40b56457055a5d373bf6d01a0cf72c3021416/gazebo/physics/World.cc#lines-747 ). \r\n\r\n## Open problems \r\n* [ ] How to specify the joint configuration in SDF (I suggest **not** to assume any predefined serialization for the joint of the robot, and instead explicitly indicate the serialization used in someway, possible mapping names to the corresponding included model using  the `hasEnding` method: https://github.com/robotology/gazebo-yarp-plugins/blob/master/libraries/common/include/GazeboYarpPlugins/common.h#L52 .\r\n* [ ] At the moment  with `gazebo-yarp-plugins` we typically `include` the model, we should check how thing works with \"nested\" models, see http://gazebosim.org/tutorials?tut=nested_model&cat=build_robot\r\n* [ ] Besides the `Load` time, it is important to also properly handle the overload of the Reset method. \r\n\r\nIf during testing one wants to build Gazebo from source, you can check https://github.com/traversaro/gazebo-superbuild or https://colcon.readthedocs.io/en/released/user/quick-start.html#build-gazebo-and-the-ignition-packages .\r\n"},"comment":{"url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins/issues/comments/439413958","html_url":"https://github.com/robotology/gazebo-yarp-plugins/issues/392#issuecomment-439413958","issue_url":"https://api.github.com/repos/robotology/gazebo-yarp-plugins/issues/392","id":439413958,"node_id":"MDEyOklzc3VlQ29tbWVudDQzOTQxMzk1OA==","user":{"login":"traversaro","id":1857049,"node_id":"MDQ6VXNlcjE4NTcwNDk=","avatar_url":"https://avatars2.githubusercontent.com/u/1857049?v=4","gravatar_id":"","url":"https://api.github.com/users/traversaro","html_url":"https://github.com/traversaro","followers_url":"https://api.github.com/users/traversaro/followers","following_url":"https://api.github.com/users/traversaro/following{/other_user}","gists_url":"https://api.github.com/users/traversaro/gists{/gist_id}","starred_url":"https://api.github.com/users/traversaro/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/traversaro/subscriptions","organizations_url":"https://api.github.com/users/traversaro/orgs","repos_url":"https://api.github.com/users/traversaro/repos","events_url":"https://api.github.com/users/traversaro/events{/privacy}","received_events_url":"https://api.github.com/users/traversaro/received_events","type":"User","site_admin":false},"created_at":"2018-11-16T14:42:30Z","updated_at":"2018-11-16T14:42:30Z","author_association":"MEMBER","body":"> For the time being, we could merge the yarpPluginConfigurationOverride plugin in the devel. I think that at the moment this solution is more general (able to change any configuration of any plugin), and doesn't require changes in other plugins structure since all the implementation is closed in the yarpPluginConfigurationOverride itself.\r\n\r\nOk, feel free to open a PR! \r\n"}}
{"actor":{"display_login":"traversaro"}}