using Teleport and FixedTimeStrategy to replay trajectory at 100Hz causes spikes in the odometry output

[maccradar]

We use the Teleport actuator to feed Morse 1.3_stable with X,Y and RZ data to replay a recorded trajectory from one of our autonomous vehicles. At the moment, we are using ATRV in Morse with an attached pose sensor and odometry sensor and run it "headless". If we plot the poses from both sensors the match perfectly with the provided input, but the actual velocity outputs from the odometry contain some weird spikes.

Here are some graphs of the recorded trajectory and velocities compared against the odometry sensor in Morse

Here is the builder script:

#! /usr/bin/env morseexec
import math
from morse.core.morse_time import TimeStrategies
from morse.builder import *
import json
import numpy as np

# Read config file
config_file = './scenarios/config.json'
json_file = open(config_file).read()
config = json.loads(json_file)
port_num = 60000

(x,y,z) = (0,0,0)

for minion in config['robots']:
    robot = ATRV(minion['name'])
    robot.translate(x,y,z)
    robot.rotate(0,0,0)
    robot.add_interface('socket')
    motion = Teleport()
    motion.add_stream('socket', port = port_num)
    port_num = port_num + 1
    motion.add_service('socket')
    robot.append(motion)

    pose = Pose()
    pose.add_interface('socket', port = port_num)
    port_num = port_num + 1
    robot.append(pose)

    odometry = Odometry()
    odometry.add_interface('socket', port = port_num)
    port_num = port_num + 1
    robot.append(odometry)

bpymorse.set_speed(100,1,1)

env = Environment('replay_sim/environments/empty.blend', fastmode = False)
env.configure_stream_manager('socket', time_sync = True, sync_port = 12000)
env.set_camera_location([20.0, -20.0, 30.0])
env.set_camera_rotation([1.05, 0, 0.78])
env.set_time_strategy(TimeStrategies.FixedSimulationStep)
env.show_physics(True)
env.show_framerate(True)

Here is relevant pymorse code from the client script:

class Robot:
    def __init__(self,robot_configuration,steps,window_size):
        self.steps = steps
        self.window_size = window_size
        self.plot = robot_configuration['plot']
        self.name = robot_configuration['name']

        # Initialise arrays to store simulator output
        self.sensor_arrays = {}

        #Init list of locks for callback
        self.sensor_locks = {}
        for sensor in robot_configuration['sensors']:
            self.sensor_arrays[sensor] = []
            self.sensor_locks[sensor] = threading.Event()
            self.sensor_locks[sensor].clear()

        #This 'sensor' is inherent to timekeeping, ie. special case
        self.sensor_arrays['new_dts'] = []

        self.csvfile = robot_configuration['csvfile']
        self.read_csv()
        self.vx_logs = []
        self.wz_logs = []

    def wait_for_locks(self):
        for __, lock in self.sensor_locks.items():
            lock.wait()

    def clear_locks(self):
        for __,lock in self.sensor_locks.items():
            lock.clear()

    def pose_callback(self, data):
        self.sensor_arrays['pose'].append(data)
        self.sensor_locks['pose'].set()

    def odo_callback(self, data):
        self.sensor_arrays['odometry'].append(data)
        self.sensor_locks['odometry'].set()

    def run_timestep(self,i):
        #read log at step i
        dist = math.sqrt(pow(self.de[i],2)+pow(self.dn[i],2))
        self.vx_logs.append(dist / self.dt[i])
        self.wz_logs.append(self.da[i] / self.dt[i])

        self.motion.publish({'x' : self.eastings[i], 'y': self.northings[i], 'z': 0.0, 'yaw' : self.angles[i], 'pitch' : 0.0, 'roll' : 0.0})

# ReplaySim class definition
class ReplaySim:
    def __init__(self, config_file, steps, window_size):
        self.steps = steps
        self.window_size = window_size
        self.lock_list = []

        for lock in self.lock_list:
            lock.clear()

        json_file = open(config_file).read()
        self.config = json.loads(json_file)

        self.robot_dict = {}
        for minion_config in self.config['robots']:
            minion = Robot(minion_config,self.steps,self.window_size)
            self.robot_dict[minion.name] = minion

    def run(self):
        step = 1
        with Morse() as simu:

            # for robot in list....
            for __, minion in self.robot_dict.items():
                simu_robot = getattr(simu, minion.name)
                minion.motion = simu_robot.motion
                minion.pose = simu_robot.pose
                minion.imu = simu_robot.imu
                minion.odometry = simu_robot.odometry
                # motion.translate(eastings[0],northings[0],0.0)
                # motion.rotate(0.0,0.0, headings[0] * math.pi / 180)
                minion.odometry.subscribe(minion.odo_callback)
                minion.pose.subscribe(minion.pose_callback)

            #endfor

            HOST = "127.0.0.1"
            self.timeControl = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
            self.timeControl.settimeout(2)
            try:
                self.timeControl.connect((HOST,12000))
            except:
                print('Unable to connect to morse clock socket')

            self.steps

            for i in range(1,self.steps-1):
                for __,minion in self.robot_dict.items():
                    minion.run_timestep(step)

                step = step+1

                #Confirm and reset callback locks
                for __,minion in self.robot_dict.items():
                    minion.wait_for_locks()
                for __,minion in self.robot_dict.items():
                    minion.clear_locks()

                self.timeControl.send(b'tick')

            for __,minion in self.robot_dict.items():
                minion.end_sim()

replay = ReplaySim('../scenarios/config.json', 1000, 1)
replay.run()
wait = input("PRESS ENTER TO EXIT.")

[severin-lemaignan]

Thanks for the very detailled issue report! That's great!

Would it be possible to also share a (part of) of the recorded trajectory, so that we can possibly turn the issue into a proper unit-test?

[maccradar]

Sure! It's actually a published dataset :-) http://its.acfr.usyd.edu.au/datasets/naturalistic-intersection-driving-dataset/ We've been using trajectory set 1 mainly.

[adegroote]

I don't test it yet, but a first issue is that the bpymorse.set_speed call is at the end of the builder script. It should be at the beginning, before the creation of the first robot.

[adegroote]

a config.json example would help too.

[maccradar]

{
  "title" : "Replay test",
  "environment_speed": "TODO",
  "fastmode": "TODO",
  "robots" : [
      {
        "name": "Robot1",
        "type": "ATRV",
        "csvfile": "intersection_dataset.csv",
        "sensors": [
          "pose",
          "odometry"
        ],
        "plot": true
      }
    ]
}

[maccradar]

Changing the location of bypymorse.set_speed has no effect on the spikes.

[maccradar]

We've noticed jumps in the Z direction of the ATRV robot when it crosses the X-axis, for no apparent reason. They seem to be happening around the timesteps where we are seeing the spikes in the linear velocity. I assume the axes don't have any thickness which is included in the physics? :-)

[adegroote]

Some code are missing to execute the client code (such as read_csv(), but maybe other stuff). Moreover, I don't know exactly what is the content of replay_sim/environments/empty.blend. If it is the same thing than standard empty.blend, you may go out of the "ground" which may lead the atrv to fall a bit, gaining some speed in some direction.

You can try to add

robot.set_no_collision()
robot._bpy_obj.game.lock_location_z = True

[maccradar]

We've enlarged the plane in the empty.blend and if you want I'll clean up our test code and commit it here. Will give the no collision a try. Before, when we were using the MotionVW, Morse crashed if you would set no_collision because of the lack of a physics engine.

[maccradar]

Thanks, @adegroote ! Your two lines solved our issue! The odometry output now perfectly follows our input. So it seemed the physics engine in combination with the Teleport is introducing strange (unrealistic) behaviour. As long as we don't need any physics, you can consider this issue closed.

[adegroote]

Yes, no_collision inhibits all physics, and default setting for VWMotion needs physics to move, so it is the expected behaviour (well, it should report an explicit error instead of crashing).