I'm writing Python scripts to program the odometry data.
I'm using one script to give velocity to the rear axle of the car.
Another non-threaded script is running to calculate a dummy position and move to a new position. When using sim.simulationTimestep, it works fine, but with a different time step, it is not tracking well.
Script working fine - (with time step equal to sim.getSimulationTimeStep())
Code: Select all
import math
def sysCall_init():
global sim, rearAxle, steering_data, rearAxleVelocity, del_t, Dummy, x0, y0, theta0, WB, startTime
sim = require('sim')
startTime = sim.getSimulationTime()
rearAxle = sim.getObject('/Rear_Axle')
Dummy = sim.getObject('/Dummy')
del_t = sim.getSimulationTimeStep()
WB = 0.58#0.58
pos = sim.getObjectPosition(Dummy, -1)
x0 = pos[0]
y0 = pos[1]
theta0 = sim.callScriptFunction('get_orient', sim.getScript(sim.scripttype_childscript, '/Sensors/Script'))
def sysCall_sensing():
global x0, y0, theta0, rearAxleVelocity, steering_data, startTime, Dummy, sim
rearAxleVelocity = (sim.getJointVelocity(rearAxle) *0.11)
steering_data = sim.callScriptFunction('get_steer_angles', sim.getScript(sim.scripttype_childscript, '/SteeringController/Script'))
steering_data = steering_data * 3.14/180
#print(rearAxleVelocity)
#print(steering_data)
theta = theta0 + ((rearAxleVelocity / WB) * math.tan(steering_data) * del_t)
x = x0 + (rearAxleVelocity * del_t * math.cos(theta))
y = y0 + (rearAxleVelocity * del_t * math.sin(theta))
up_pos = [x, y, 0.33]
sim.setObjectPosition(Dummy, -1, up_pos)
x0 = x
y0 = y
theta0 = theta