search

ubi-agni / mujoco_ros_pkgs

Wrappers, tools and additional API's for using ROS with MuJoCo
68 stars 12 forks source link

Fix controller reset #11

Closed rhaschke closed 1 year ago

rhaschke commented 1 year ago

After some research, I found that if, and only if, /use_sim_time is set in the launch file before starting any node, the TF buffer is cleared upon a jump back in time and the joint_state_publisher node re-publishes joint transforms. However, control still seems unable to handle this. I could not find the reason, but sometimes upon reset the robot stays in position, i.e. the controller sends a valid control signal, and sometimes the robot just drops to the floor without any signs of being actively controlled. Once the point in (ROS) time is reached, where the reset was previously issued, the robot starts being actuated again.

There were two remaining issues:

  1. reset_ctrls wasn't initialized sometimes: https://github.com/ubi-agni/mujoco_ros_pkgs/blob/d4463dbf7ac5b7224c8f35ff8a341e39e27dfc39/mujoco_ros_control/src/mujoco_ros_control_plugin.cpp#L159
  2. Resetting of controller times was done in main thread, while evaluation was done in another. This randomly resulted in the reset times being overwritten by the latter thread again, thus not causing a reset, but to a negative time difference, effectively disabled the call to robot_hw_sim_->writeSim(): https://github.com/ubi-agni/mujoco_ros_pkgs/blob/d4463dbf7ac5b7224c8f35ff8a341e39e27dfc39/mujoco_ros_control/src/mujoco_ros_control_plugin.cpp#L175-L176

Occasionally TF_OLD_DATA and TF_REPEATED_DATA warnings are still thrown:

[ WARN] [1665881466.432722731] [ros.robot_state_publisher] [/robot_state_publisher]: Moved backwards in time: 1.668000000 -> 0.000000000 msg: 0.000000000
[ INFO] [1665881466.433811401] [ros.mujoco_ros_control.mujoco_ros_control] [/mujoco_server]: Resetting mujoco_ros_control due to time reset
[ WARN] [1665881466.440837947, 0.007000000] [ros.tf2_ros] [/mujoco_server]: Detected jump back in time of 1.652s. Clearing TF buffer.
[ WARN] [1665881468.112881338, 1.678000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame panda_leftfinger at time 1.667000 according to authority unknown_publisher
[ WARN] [1665881468.112925709, 1.678000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame panda_rightfinger at time 1.667000 according to authority unknown_publisher
[ WARN] [1665881468.112960183, 1.679000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame panda_link1 at time 1.667000 according to authority unknown_publisher
...
[ WARN] [1665881468.113142298, 1.680000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server]: TF_REPEATED_DATA ignoring data with redundant timestamp for frame panda_link7 at time 1.667000 according to authority unknown_publisher

I didn't manage to fix those yet. This seems to be a timing issue (of course). When compiling with Debug mode instead of RelWithDebInfo the error vanishes. All those messages originate from the robot_state_publisher processing the latest joint_states message before the time reset. But I don't see a reason (and evidence) that this message (and the triggered TF msgs) are published twice!

rhaschke commented 1 year ago

Why, in first place, do you need the TF buffer in MuJoCo. The only usage is here: https://github.com/ubi-agni/mujoco_ros_pkgs/blob/cc244877bd2584ff1fd41b807679c2d769970174/mujoco_ros/src/mujoco_sim.cpp#L2436

... to transform setModelState requests not given in world frame. By the way, this API should use std_msgs::Header for the reference frame (because you also need a timestamp together with the reference frame!)

rhaschke commented 1 year ago

Occasionally TF_OLD_DATA and TF_REPEATED_DATA warnings are still thrown:

Maybe I found a solution to that: https://github.com/ros/geometry2/pull/542. Please report any left-over warnings!

rhaschke commented 1 year ago

Looks good to me.