spawn_model -J initial joint positions not working

[davetcoleman]

using the command line argument -J

i spent a lot of time investigating this today to no avail

@hsu

[davetcoleman]

Related to https://bitbucket.org/osrf/gazebo/pull-request/628/allow-scoped-and-non-scoped-joint-names-to/diff

[davetcoleman]

The ability to specify initial joint positions is still broken in the current gazebo_ros_pkgs - any idea why @hsu? Thanks!

[hsu]

seems to be working for me using the double pendulum example:

rosrun gazebo_ros spawn_model -sdf -model p100 -file ~/.gazebo/models/double_pendulum_with_base/model.sdf -x 2 -J upper_joint -1.5

Can you confirm it's still broken? thanks.

[davetcoleman]

Its still broken with Baxter using a URDF, but your SDF version does work on my computer using the double pendulum example. To reproduce, first get the Baxter URDF:

git clone https://github.com/davetcoleman/baxter_common.git -b dual_parallel_grippers

Then (replace the file location of the URDF):

rosrun gazebo_ros gazebo
rosrun gazebo_ros spawn_model -urdf -model baxter -file ~/ros/ws_baxter/src/baxter_common/baxter_description/urdf/baxter.urdf -z 0.93 -J right_s0 -0.272659 -J right_s1 1.04701 -J right_e0 -0.00123203 -J right_e1 0.49262 -J right_w0 -0.0806423 -J right_w1 -0.0620532 -J right_w2 0.0265941 -J left_s0 0.192483 -J left_s1 1.047 -J left_e0 0.000806359 -J left_e1 0.491094 -J left_w0 -0.178079 -J left_w1 -0.0610333 -J left_w2 -0.0124707

This should start baxter with no potential energy in its joints (arms down), but instead I see Baxter's hands straight out on both sides and they fall when the simulation starts.

[hsu]

Not sure why yet. Calling set configuration through rosservice seems to work after the model has been spawned:

rosservice call /gazebo/set_model_configuration '{ model_name: "baxter", urdf_param_name: "robot_description", joint_names: [ "right_s0", "right_s1", "right_e0", "right_e1", "right_w0", "right_w1", "right_w2", "left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2" ], joint_positions: [ -0.272659, 1.04701, -0.00123203, 0.49262, -0.0806423, -0.0620532, 0.0265941, 0.192483, 1.047, 0.000806359, 0.491094, -0.178079, -0.0610333, -0.0124707 ] }'

must be something in the spawn service.

Side note, I had to modify your urdf to replace note: --> note and replace tabs by spaces, otherwise, I get these types of errors (line/column might be off from urdf in repo):

$ rosparam load `rospack find baxter_description`/urdf/baxter.urdf /robot_description
...
yaml.scanner.ScannerError: mapping values are not allowed here
  in "<string>", line 34, column 14:
        <!-- note: invisible -->
                 ^

or

$ rosparam load `rospack find baxter_description`/urdf/baxter.urdf /robot_description
...
yaml.scanner.ScannerError: while scanning for the next token
found character '\t' that cannot start any token
  in "<string>", line 34, column 1:
            <!-- note invisible -->
    ^

[dpsolomon]

Was there ever any resolution to this problem?

Could it have been a namespace problem induced by using a urdf to load the model instead of an sdf file?

[dpsolomon]

I take back the namespace comment. Still, was there ever a solution to this?

[hsu]

Hi Daniel,

what version of gazebo are you using? Could be related to this bug below?

https://bitbucket.org/osrf/gazebo/issue/1138/set-joint-positions-in-jointcontroller

John

On Thu, Aug 7, 2014 at 11:54 PM, Daniel Solomon notifications@github.com wrote:

I take back the namespace comment. Still, was there ever a solution to this?

— Reply to this email directly or view it on GitHub https://github.com/ros-simulation/gazebo_ros_pkgs/issues/93#issuecomment-51569115 .

[davetcoleman]

AFAIK it was never fixed

[dpsolomon]

I'm using the debian version 1.9.5 compatible with ROS hydro. Is there any workaround? Like Dave, I'm just trying to set the initial joint position directly (during roslaunch).

[hsu]

seems to work with gazebo 4.x release. I am testing in branch issue_93_set_joint_position with

roslaunch gazebo_ros simple_arm_world.launch

testing gazebo 1.9 branch next.

[fmessmer]

I made the following observations when trying to use -J arguments with spawn_model: (Setup: Trusty, Indigo, gazebo 2.2.3-1~trusty, gazebo_ros_pkgs/indigo-devel from source)

The test file in the gazebo_ros_pkgs/issue_93_set_joint_position branch works fine!

Then I tried to use the -J arguments with the universal robot UR5 (https://github.com/ipa-fxm/universal_robot/tree/test_initial_joint_position - this branch is up-to-date with the official indigo-devel branch + -J arguments in the ur_gazebo ur5.launch file). However, when starting roslaunch ur_gazebo ur5.launch the -J arguments are not applied and the UR5 is still in the "all joints 0.0" configuration. When I saw that the test files from gazebo_ros_pkgs/issue_93_set_joint_position branch were not using any transmission, had no gazebo_ros_control plugin loaded and no ros_controller started, I removed all that for the UR5 as well - without success. Even starting gazebo in paused mode did not work. In the end, I "accidentially" ran rosrun gazebo_ros spawn_model -urdf -param robot_description -model ur5 -z 0.1 -J shoulder_pan_joint 0.0 -J shoulder_lift_joint 0.0 -J elbow_joint -0.4 -J wrist_1_joint 0.0 -J wrist_2_joint 0.5 -J wrist_3_joint 0.0 in a separate terminal, although the model was already loaded in gazebo via the launch file - and this "respawned" the model with the -J arguments applied! Please note, that the arguments in the rosrun command are the same as given in the launch file!

In the end, I added all the transmissions, gazebo_ros_control plugin and ros_controllers stuff again and started gazebo in "running" mode.....after running a second spawn_model, the -J arguments are applied to the robot!

I have not yet investigated further what causes this behavior, but I wanted to document this here! Can someone confirm this observations? Any thoughts?

[hsu]

I also noticed that while the simple model worked fine, other models may not work. Maybe a race condition somewhere at start up? Will look into this in more detail.

[fmessmer]

It looks like everything is fine up to https://github.com/ros-simulation/gazebo_ros_pkgs/blob/indigo-devel/gazebo_ros/src/gazebo_ros_api_plugin.cpp#L1472 The -J arguments are handed over to joint_position_map correctly!

[hsu]

I wonder if it's because Model::jointController hasn't been initialized yet when this function is called.

[hsu]

please give gazebo branch set_joint_position_on spawn (c8d353e) a try? thanks.

[fmessmer]

Hmm, the branch seems to work for the test you added. However, testing it with the universal_robot test-branch running roslaunch ur_gazebo ur5.launch doesn't seem to work...maybe there is something more when having transmission in the urdf and controllers running...

[jbohren]

please give gazebo branch set_joint_position_on spawn (c8d353e) a try? thanks.

Is there any way to do something similar for Gazebo 2.2.x?

[jbohren]

Also w.r.t. 99b9dc5, it doesn't always work.

[fmessmer]

I also did some experiments with issue_93_set_joint_position_hack_indigo branch.

It seemes to work (most of the time) for "small" models, e.g. just a manipulator (e.g. Univeral UR5). However, with a more complex robot like our rob@work (where we have a single top-level launch file that takes care of everything like upload parameters, start gazebo, spawn_model (base, manipulator, sensors), start_controllers and other stuff), the initial config is not considered correctly.

It appears to me that it also depends on the point in time at which the controllers are started...?

[k-okada]

99b9dc5 works for me, This will solve our problem posted on https://groups.google.com/a/rethinkrobotics.com/forum/#!topic/brr-users/QWITm3lfxvM, thanks!

I'm testing on setting baxter gazebo simulation using baxter_gazebo.install of indigo-devel of gazebo_simulator package on indigo.

[j-rivero]

see #428

[hsu]

another possibility is Model::SetJointPositions is called before the model has had a chance to spawn completely. There needs to be a check after checking if model exists to see if model finished spawning completely.

Maybe instrument a signal at the end of ProcessModelMessages to confirm model has been created.

[TheDash]

Any thoughts/resolutions on this? Is there still no way to set a joint position in gazebo other than by initial URDF config?

[hsu]

here's a starting point proposal for handling this upstream in Gazebo first:

• add initial_position in sdf for joint blocks. Still need to do:
  • add support in urdfdom and urdfdom_headers, or
  • update parser_urdf.cc to support initial_position as part of gazebo extensions.
• process initial joint position, need to add tests to this branch. Also need to add examples and tutorials.

To try it out, run

gazebo --verbose -u [gazebo]/test/worlds/initial_joint_position.world 

[stoic-roboticist]

Even though I have the fix proposed in https://github.com/ros-simulation/gazebo_ros_pkgs/commit/99b9dc598413ab37f6d5f7e67b005f195d88ccdc, it was still not working for me. However, starting gazebo in paused mode allowed me to spawn the model with the specified joint angles. I hope this observation is of any help.

[corot]

Is this issue fixed on Kinetic-Gazebo7? -J option doesn't work for me, unless I do as @hatem193 suggests and start Gazebo paused. But I would like a confirmation, as I'm already having a bunch of other problems after migration (the arms starts partially tilted, as obeying gravity, instead of the all-zeros right-up default pose)

[Nic-G]

I see similar behaviour, using Kinetic-Gazebo7: as noted by @ipa-fxm above, -J works for a single UR5, but does not work reliably for a more complex robot. Starting Gazebo paused and using /set_model_configuration works fine

[davetcoleman]

It seems in the kinetic-devel branch the "last resort" commit has been merged in and outputs ugly console info:

rosout: temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition.

What is the status of fixing this long-standing issue?

[TheDash]

@Nic-G What is your syntax for getting set_model_configuration to work?

I'm using

rosservice call /gazebo/set_model_configuration mobile_base robot_description [l_ur5_arm_shoulder_link] [1.57]

But no avail..

[Nic-G]

@TheDash - this worked for me: rosservice call /gazebo/set_model_configuration '{ model_name: "robot", urdf_param_name: "robot_description", joint_names: [ "l_ur5_shoulder_lift_joint", "l_ur5_shoulder_pan_joint" ], joint_positions: [ -1.7296, -0.8471 ] }'

[davetcoleman]

The issue isn't the syntax, but rather a race condition where sometimes the set model configuration doesn't work without a nasty sleep timer

[Nic-G]

@davetcoleman - good point. @TheDash - I have an initialisation ROS node which sleeps for a couple of seconds to allow Gazebo model setup to complete, then I call /set_model_configuration as above, then I un-pause. That seemed to work reliably, though a proper check would obviously be preferable to an arbitrary 2s sleep!

[TheDash]

There must be something else going on here. I have ros_control running for the joints to actuate them and the controllers are running. No matter what values/names i put inside the rosservice call nothing will work with physics paused.

This is with a manual call after about 30 seconds of having the sim running.

[TheDash]

I am however, getting this one weird warning in the Gazebo node.

updateConfig() called on a dynamic_reconfigure::Server that provides its own mutex. This can lead to deadlocks if updateConfig() is called during an update. Providing a mutex to the constructor is highly recommended in this case. Please forward this message to the node author.

Could this possibly be it?

[TheDash]

I finally managed to get this work by upgrading from Gazebo2 to Gazebo5, and then calling it using the command above.

The only way I found this out was by trying to manually set things through the interface and then I got this error in the terminal, which tipped me off:

Warning [ModelListWidget.cc:701] Model modification is currently disabled. Look for this feature in Gazebo 2.0

Now calling that command spawns it how I want

[TheDash]

I don't think set_model_configuration is working as intended. When you do this and you have ros_control plugins included in your gazebo urdf, soon as physics are unpaused it goes back to its original position.

I'm not sure this is intended functionality @davetcoleman

[TheDash]

After playing with this for about 6 hours.. I found the correct sequence of events to make sure this happens correctly and make it "working as intended" with ros control

1. Start gazebo paused
2. Load controllers stopped
3. Spawn model
4. Call the set model configuration rosservice call
5. Unpause physics (because controllers cannot be loaded/unloaded if physics is stopped)
6. Load controllers

and voila, you get a robot spawned in your working configuration with the controllers ready to go.

[k-okada]

@TheDash Great!, Do you think we can create script node which control this sequence?

[davetcoleman]

@TheDash I don't understand why you need to follow all those steps - even Baxter in Gazebo 2 / Indigo the spawn to position worked correctly except it uses the sleep timer to avoid race conditions

[TheDash]

@davetcoleman I found out that the sleep timer to avoid race conditions works if ros control isn't running. Otherwise, ros control must be updating too fast and just resets it to its previous position and I have no idea how to command it to stop other than unloading the controllers/have them stopped.

Although, im still getting bugs wherein the model configuration goes to random positions sometimes.. This is pretty dang buggy. When I launch my entire stack from a launch file I have about a 30% success rate for which this sequence sets it (IF I SPAWN EVERYTHING w/ GAZEBO PHYSICS RUNNING). When Gazebo physics is spawned stopped, it works as defined above.

I have no idea why this rosservice is so buggy, or requires so much prework to use. You'd think that calling this service at any point/irregardless of any plugins that it would just set the robot to that model config.

I think that's the larger issue we have to fix here in the future

[TheDash]

@k-okada

!#/bin/bash

rosservice call /gazebo/pause_physics rosservice call /controller_manager/switch_controllers ....... (you have to tell it to stop whatever controllers youre running here w/ a crafted msg) rosrun gazebo_ros spawn_model .... rosservice call /gazebo/set_model_configuration ... rosservice call /gazebo/unpause_physics rosservice call /controller_manager/switch_controllers ...... (same as above)

Ideally I guess, the script could read whatever robot_description you have, and take in a .yaml file of the joints mapped for model config, while also stopping the controllers of what is detected by the system.

I think we should make a PR and just fix the set_model_configuration service call to do this tbh.

[TheDash]

Ok this race condition bug is seriously annoying me. Do we have to make a PR to gazebo code to fix this? I'm like 10 minutes short of making a plugin to handle interfacing with gazebo code directly which will allow me to set joint positions without controllers screwing everything up on gazebo start.

[mrjogo]

FWIW, calling /gazebo/pause_physics then /gazebo/reset_simulation before spawning with joint positions also seems to work, but the reset might wreak havoc with your other nodes. (I have to start Gazebo unpaused because the node that composes my URDF relies on a ROS Timer to start)

[Kukanani]

Correct me if I'm wrong, but this same issue is also causing problems when trying to do either a "hard reset" (using /gazebo/reset_simulation) or a "soft reset" (calling /gazebo/set_model_configuration after the controllers have been loaded).

here's a Gazebo answers post that claims that set_model_configuration works as expected in Gazebo 7, but I'm experiencing the same issue as @TheDash reported, where the ros_control plugin resets the position of the joints after physics are started again.

[rkeatin3]

@TheDash, I'm also running into the problem of having joints reset their positions from commanded value on physics unpause.

Did you end up writing a Gazebo plugin in which to set the joint positions? If so, is that method of setting them not susceptible to the same problem with ros_control?

[TheDash]

I solved this using another method. The whole gazebo system is pretty scuffed. I wrote an entirely proprietary application which this solution is now apart of.

Anyways, it was solved by using dynamic reconfigures joint position sliders updating the hard coded urdf. Basically the best way to go about this is to just change the hard coded urdf unless you want to deal with a crapton of race conditions.

On Mon, Oct 2, 2017 at 5:25 PM rkeatin3 notifications@github.com wrote:

@TheDash https://github.com/thedash, I'm also running into the problem of having joints reset their positions from commanded value on physics unpause.

Did you end up writing a Gazebo plugin in which to set the joint positions? If so, is that method of setting them not susceptible to the same problem with ros_control?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/ros-simulation/gazebo_ros_pkgs/issues/93#issuecomment-333669971, or mute the thread https://github.com/notifications/unsubscribe-auth/ABtC6V7ZwUCO2FtQsFFkypoon451hnj0ks5soVTTgaJpZM4A3Eap .

--

Devon Ash <a href="http://ca.linkedin.com/pub/devon-ash/48/478/981" style="text-decoration:none;"><img src=" https://static.licdn.com/scds/common/u/img/webpromo/btn_in_20x15.png" width="20" height="15" alt="View Devon Ash's LinkedIn profile" style="vertical-align:middle" border="0">View Devon Ash's profile

[rkeatin3]

Perhaps I'm wrong here (since there is a lot of code to digest to fully understand the ros_control pipeline), but it seems like the problem is likely in DefaultRobotHWSim:

The gazebo_ros_control plugin is loaded here when the model is spawned in Gazebo (meaning starting Gazebo paused doesn't help). The plugin initializes DefaultRobotHWSim for all of the transmissions in the URDF, with a default value of 0 for all the elements of joint_position_command_ here.

I don't think I am qualified to suggest a fix, but am I correct in thinking that this is the wrong behavior? I would expect that all joints without controllers running should be unactuated (i.e. be 'skipped' inside writeSim(...)).

[rkeatin3]

Well I implemented some changes that effectively do 'skip' joints without active controllers loaded, but they don't result in the behavior I was expecting: instead of the robot behaving like a ragdoll, the joints drift slowly from their starting position.

Any ideas on why this is the behavior? I tried setting the force on the JointPtr to 0, tried calling Update() on the joint, but none of what I tried worked...

[jacknlliu]

@TheDash I found rosservice call /controller_manager/switch_controllers must be called before rosservice call /gazebo/pause_physics, or switch_controllers will block.

@j-rivero And any ideas about this? This is also related how to reset the gazebo world with ros control correctly, not keep joint position to the previous state.

And /gazebo/set_model_configuration still work randomly.

I'm using ROS kinetic and gazebo 7 with Ubuntu 16.04.