Error accumulation when reverting objects several times

[fabienrohrer]

It has been reported several times that there is some error accumulation when reverting articulated objects several times using our proposed methods (Supervisor setters and revert functions).

Notably, the support ticket 9747:

After running my simulation for a long time, including repositioning the Thymio robots several time, the Thymio robots disassemble themselves. The wheels are distanced to the Thymio body (see attached picture). In the program, robots can either drive straight or turn on the spot. Robots might constantly turn for hours. As I'm using the fast mode and this effect only happens after several hours, I couldn't point out where the problem exactly starts. I tried reproducing it by letting a single Thymio turn for a long time, but no problems occurred in that case.

As this prevents to run numerical optimization methods smoothly, it is urgent to address this issue, and prevent it comes back with tests.

[MentalGear]

Glad this issue is prioritiozed, as simulation consistency is essential for ML. On the grounds of testing, besides normal unit tests, it might also be interesting to build an integrated test into normal runs, where the distance between robot parts/joints is measured on intialisation, and then on every xth (1000 or whatever) timestep, it is checked if the distance is still the same, and if not a warning is raised in the console.

[omichel]

I suspect that when you reposition the robots, some robots are overlapping each other. If that's the case, the physics engine will generate strong constraints as a result of collision detections that will likely break the position of the wheels of the robots. Are you sure that when you reposition the robots, you check that the new location of a robot is free, so that no collision with another robot or an object occurs?

[DavidMansolino]

I have tried with a very simple scenario: 1 e-puck with braitenberg controller in a square arena. With 2 use cases:

1. Every 6.4 seconds the simulation is reset.
2. Every 6.4 seconds the robot is moved to a random location and orientation (only around Y axis) and making sure it does not touch the walls.

World:

#VRML_SIM R2019b utf8
WorldInfo {
}
Viewpoint {
  orientation -0.7554567155273139 0.5554918089578958 0.3474392049630664 1.2114128455600468
  position 0.7238033733266418 1.7258587941508157 1.0161681104939888
}
TexturedBackground {
}
TexturedBackgroundLight {
}
RectangleArena {
  floorSize 1.1 1.1
}
DEF ROBOT0 E-puck {
  translation 0.142818 0 -0.485123
  controller "braitenberg"
}
Robot {
  controller "reset_supervisor"
  customData "0"
  supervisor TRUE
}

Controller:

"""reset_supervisor controller."""

from controller import Supervisor

import random

supervisor = Supervisor()

timestep = int(supervisor.getBasicTimeStep())

iteration = int(supervisor.getCustomData())
supervisor.setLabel(0, 'Iteration: %d' % iteration, 0.2, 0.2, 0.1, 0xFF0000, 0)

robotNodes = []
robotTranslationFields = []
robotRotationFields = []

for i in range(20):
    robotNode = supervisor.getFromDef('ROBOT%d' % i)
    if robotNode is None:
        break
    robotNodes.append(robotNode)
    robotTranslationFields.append(robotNode.getField('translation'))
    robotRotationFields.append(robotNode.getField('rotation'))

# Case 1: robot reset
# iteration += 1
# supervisor.setCustomData(str(iteration))

while supervisor.step(200 * timestep) != -1:
    # Case 1: simulationReset
    # supervisor.simulationReset()

    # Case 2: robot reset
    for i in range(len(robotNodes)):
        robotTranslationFields[i].setSFVec3f([random.uniform(-0.5, 0.5), 0.0, random.uniform(-0.5, 0.5)])
        robotRotationFields[i].setSFRotation([0.0, 1.0, 0.0, random.uniform(0.0, 6.283185)])
        robotNodes[i].resetPhysics()
    iteration += 1
    supervisor.setLabel(0, 'Iteration: %d' % iteration, 0.2, 0.2, 0.1, 0xFF0000, 0)
    supervisor.setCustomData(str(iteration))
    # if iteration > 100: 
        # supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)

With simulationReset it seems to work fine, but with the random re-location there are issues. First, after a high number of iterations the wheels are clearly drifting: And then (which might be the cause of the issue), when resetting the location, the robot is not at all at the location it is supposed to be (at least for the height), that's maybe an issue with contact points not updated:

Here is my test scenario: revert_bug.zip

This scenario might not be the best one as some users reported that the issue is raised after ~10iterations only (which is clearly not the case in this scenario).

Edit: I have updated the supervisor controller to support mutli-robots (but relocation security distance is not implemented yet).

[DavidMansolino]

I have been able to improve the scenario to raise the issue after only ~10iterations, the braitenberg controller is too 'smooth' using a more abrupt controller in term of speed is causing much more problems:

from controller import Robot, Motor
import random

TIME_STEP = 64

MAX_SPEED = 6.28

# create the Robot instance.
robot = Robot()

# get a handler to the motors and set target position to infinity (speed control)
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))

while robot.step(TIME_STEP) != -1:
    leftMotor.setVelocity(random.uniform(-MAX_SPEED, MAX_SPEED))
    rightMotor.setVelocity(random.uniform(-MAX_SPEED, MAX_SPEED))

Here is the updated worlds (single and multi robots) and controllers: revert_bug.zip

[DavidMansolino]

These offsets are not hidden into ODE but are clearly visible in the scene-tree:

Should be:

translation -0.026 0.02 0
rotation 1 0 0 0

[joangerard]

I experimented similar issues... those wheel translation values, in my case, should be 0.045 and 0.025. My robot is programmed to sometimes turn randomly and in that case the simulator shows that the wheel translation values are modified then the robot translation field is reset (while robot is turning) and the wheel translation values are never reset.

[fabienrohrer]

Interesting fact, the bug seems to disappear if I simply reset the motor speed some steps before to revert the node. Modifications on the controller:

from controller import Robot
import random

MAX_SPEED = 6.28

# create the Robot instance.
robot = Robot()
timestep = int(robot.getBasicTimeStep())

# get a handler to the motors and set target position to infinity (speed control)
leftMotor = robot.getMotor('left wheel motor')
rightMotor = robot.getMotor('right wheel motor')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))

time_slot = 0.001 * 200 * timestep

while robot.step(timestep) != -1:
    t = (robot.getTime() % time_slot) / time_slot

    if t > 0.8:
        leftMotor.setVelocity(0.0)
        rightMotor.setVelocity(0.0)
    else:
        leftMotor.setVelocity(random.uniform(-MAX_SPEED, MAX_SPEED))
        rightMotor.setVelocity(random.uniform(-MAX_SPEED, MAX_SPEED))

[fabienrohrer]

In this case, contact points are present (I also dropped the body sleep feature to test), but not internal velocities.

[fabienrohrer]

I inspected the physics dump just after the physics reset, and it seems to be very clean (velocity parameters are reset, joints are precisely set).

I inspected the contact points (from the dump, by removing them on reset, and also by trying to prevent their creation), the issue remains in any case.

[fabienrohrer]

Other interesting fact, the issue is reduced a lot when the WorldInfo.basicTimeStep is set to 8. Rather than occurring after 10 iterations, it appears after 500 iterations.

[fabienrohrer]

The issue is that joint positions are not reseted smoothly in resetPhysics() functions.

[fabienrohrer]

Fixed in #1137

[joangerard]

Just a comment on this. I still need to call resetPhysics() method; otherwise, the error still happens.