search

DavidPL1 / assembly_example

Example code for interaction with our assembly simulation ICRA 2023 challenge
7 stars 1 forks source link

Spawning objects at different random positions without re-launching container #6

Closed abhishek47kashyap closed 1 year ago

abhishek47kashyap commented 1 year ago

For difficulty_level 2 or higher, I'm looking for a way to programmatically spawn objects on the table at different random poses than before, as if doing a fresh start. For example, in the context of screwing task, I want to make the 3 objects on the table - nut, screw, fixture - re-spawn at different locations without re-launching s4dx/assembly_server. Is this currently supported in any way?

I need to do some data collection and am thinking of going about it like so:

start s4dx/assembly_server in a terminal

// in a python script
do N times:
    move robot, record data, etc.
    respawn objects at different locations

On making a request to the service /mujoco_server/reset, the objects get spawned at the same random locations as when s4dx/assembly_server started (verified this by echoing a GT topic, like /nut_pos_GT). I'm assuming this is by design, which is fine, but for my purposes, I'm looking for a way to make those objects spawn at different random areas than before.

The service /mujoco_server/shutdown, true to its name, takes everything down and I've to re-launch s4dx/assembly_server to get the environment back up again (the objects spawn at different locations, as expected). I guess I could hack something together to docker run s4dx/assembly_server programmatically, however before spending time doing all that I wanted to make sure if what I'm looking for already exists or will be made available in a future release.

DavidPL1 commented 1 year ago

This is currently not possible, but is rather easy to implement. This would be implementable as a service call to the assembly_manager node. I can try to include it in the upcoming release

abhishek47kashyap commented 1 year ago

It'll be great for this to be available as I think several participants might benefit from it.

DavidPL1 commented 1 year ago

This feature was added in the 2.0 release (see #7).

The example does not seem to be 100% stable when I run it in on my low resources laptop, as sometimes tearing down the controller manager causes the simulation to hang. I'm not sure where this comes from (might be related to not destroying the controller manager interface during the call to the reload service), but I didn't want to delay the new release any longer. Please get back to me on this, if you also experience issues while reloading.

abhishek47kashyap commented 1 year ago

Thanks for exposing the two services. I could verify expected behavior of /reload_with_new_scene and /reload_with_old_scene.

As time gets reset to 0, there is ROSTimeMovedBackwardsException occurring in thread Thread-24 when controllers are reloaded. This does not cause me any problems and I'm able to continue from that point on. Here's an example traceback:

// GT position before scene reset
Nut GT position
x: 0.4902816712856293
y: 0.24714747071266174
z: 0.3339983820915222

// my own logs as I make a request for scene reset, followed by a request to /user_ready srv, and then reload controllers
[INFO] [1680010450.034303, 3.216000]: Reloading with new scene..
[INFO] [1680010450.043691, 3.216000]: Scene reload successful
[INFO] [1680010450.046341, 3.216000]: Waiting for service /user_ready
[INFO] [1680010453.073474, 3.264000]: .. found, firing request
[INFO] [1680010453.079251, 3.264000]:    good to go!
[WARN] [1680010453.084289, 3.264000]: Reloading controllers ..
Exception in thread Thread-24:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
    self.run()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 228, in run
    r.sleep()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 103, in sleep
    sleep(self._remaining(curr_time))
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 163, in sleep
[ WARN] [1680010453.134106932, 0.037000000]: Detected jump back in time of 3.227s. Clearing TF buffer.
    raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
[WARN] [1680010453.135505, 0.037000]: Detected jump back in time of 3.227000s. Clearing TF buffer.
rospy.exceptions.ROSTimeMovedBackwardsException: ROS time moved backwards
Loaded 'franka_gripper'
Loaded 'franka_state_controller'
Loaded 'effort_joint_trajectory_controller'
Started ['franka_gripper', 'franka_state_controller', 'effort_joint_trajectory_controller'] successfully

// GT position after scene reset
Nut GT position
x: 0.5317687392234802
y: 0.21889562904834747
z: 0.3339976966381073

However, if I add a sleep (rospy.sleep(4)) after reloading the scene but before reloading the controllers, that same traceback crashes my script. Here are example logs (mostly similar to above):

// GT position before scene reset
Nut-head GT position
x: 0.5168301463127136
y: 0.28864455223083496
z: 0.35999804735183716

// my own logs as I make a request for scene reset, followed by a request to /user_ready srv, and then reload controllers
[INFO] [1680013969.717716, 2.077000]: Reloading scene by calling /reload_with_new_scene..
[INFO] [1680013969.724037, 2.077000]: Scene reload successful
[INFO] [1680013969.725453, 2.077000]: Waiting for service /user_ready
[INFO] [1680013973.053424, 2.077000]: .. found, firing request
[INFO] [1680013973.060215, 2.077000]:    good to go!
[INFO] [1680013973.063726, 2.077000]: Sleeping a bit before controller reloading
Exception in thread Thread-24:
Traceback (most recent call last):
  File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
Traceback (most recent call last):
  File "/root/HSR/vmc_icra_23/src/assembly/scripts/screwing_task.py", line 52, in <module>
    self.run()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 228, in run
    dl_one.scene_reload()
  File "/root/HSR/vmc_icra_23/src/assembly/scripts/dl_one.py", line 385, in scene_reload
    r.sleep()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 103, in sleep
    sleep(self._remaining(curr_time))
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 163, in sleep
    rospy.sleep(2)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 163, in sleep
    raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
rospy.exceptions.ROSTimeMovedBackwardsException: ROS time moved backwards
    raise rospy.exceptions.ROSTimeMovedBackwardsException(time_jump)
rospy.exceptions.ROSTimeMovedBackwardsException: ROS time moved backwards
[ WARN] [1680013973.110325243, 0.037000000]: Detected jump back in time of 2.04s. Clearing TF buffer.
[WARN] [1680013973.111027, 0.033000]: Detected jump back in time of 2.044000s. Clearing TF buffer.

// script has exited and there's the terminal prompt

Not entirely sure how an intervening sleep is causing this, but thought I'd put it out here, in case this proves useful.. :man_shrugging:

Closing this issue as the feature request has been made available (thanks again!)

DavidPL1 commented 1 year ago

Not entirely sure how an intervening sleep is causing this, but thought I'd put it out here, in case this proves useful.. :man_shrugging:

I just tested what happens exactly. When you reset the scene but don't call /user_ready, mujoco_ros does not send a clock message signalling that time was reset, so when you invoke rospy.sleep the current cached time > 0 is used and only once /user_ready is called and the simulation resumes stepping the new time is being published. This then causes your rospy.sleep call to notice that time moved backwards and panic.

I should probably add a time reset clock message on reload to fix that.