Python: How to create trajectory between start and end positions with specified speeds in a vectorized way?

Question

I am trying to find a way to create a trajectory between two points with specified speeds in a vectorized way. For example start point is [0, 0] and end point is [25, 15]. Next I need to specified speeds [25, 4, 8, 2] and their corresponding probabilities [0.4, 0.2, 0.3, 0.1]. So for each time interval, speed can be 25 m/s (with probability 40%), or 4 m/s (probability 20%), etc.

Here is an example of desired output:

[[0,0], [3,1], [6,3.5], [12,7], [14,8], [19,11], [25,15]]

As you see object moved from [0,0] to [3,1] with speed e.g. 4 m/s, next from [3,1] to [6,3.5] with speed e.g. 8 m/s, etc.

(note it is just example with approximate coordinates)

Here is my attempt to create a script to generate such trajectories:

from math import asin, degrees

ue_speed = [3, 4, 8, 25]
ue_speed_prob = [0.4, 0.2, 0.3, 0.1]
steps = 20 # this parameter hardcoded and should be removed
square_size = 100 # need for scaling
time_interval = 100 # need for scaling
start_coord = [0, 0]
end_coord = [25, 15]
b = end_coord[0] - start_coord[0]
a = end_coord[1] - start_coord[1]
c = (a**2 + b**2)**0.5
theta =  [degrees(asin(a/c))] * (steps - 1)
start = [start_coord]

v = np.random.choice(ue_speed, size=steps-1, p=ue_speed_prob)
R = np.expand_dims(((v * time_interval) / square_size), axis=-1)
xy = np.dstack((np.cos(theta), np.sin(theta))) * R
trajectory_ = np.hstack((np.zeros((1, 1, 2)), np.cumsum(xy, axis=1)))
trajectory = np.abs(trajectory_[0] + start )

Output:

array([[ 0.        ,  0.        ],
       [ 2.69850332,  1.31075545],
       [ 5.39700664,  2.62151089],
       [ 8.09550996,  3.93226634],
       [10.79401327,  5.24302179],
       [14.3920177 ,  6.99069571],
       [21.58802655, 10.48604357],
       [24.28652987, 11.79679902],
       [26.98503318, 13.10755446],
       [30.58303761, 14.85522839],
       [33.28154093, 16.16598384],
       [35.98004425, 17.47673929],
       [38.67854756, 18.78749473],
       [45.87455641, 22.28284259],
       [49.47256084, 24.03051652],
       [56.66856969, 27.52586437],
       [59.36707301, 28.83661982],
       [62.06557632, 30.14737527],
       [65.66358075, 31.8950492 ],
       [69.26158517, 33.64272312]])

Output is not correct. End point should be [25, 15].

Is it possible to change somehow code above to generate correct results?

Jakob Stark · Accepted Answer

First I think you were fooled by a simple but yet subtle mathematical error you made. In your illustration, you show the first path segment to be from (0,0) to (3,1). This however does not correspond to a "speed" of 4, as your object moves along the diagonal path. For the first line segment you would get a speed of

v = (1**2 + 3**2)**0.5 (= 3.162)

Even less obvious to me is, how you got to a speed of 4 in the second line segment, but you mentioned approximate coordinates, so I will assume, that these are not the exact coordinates that you are looking for.

That put aside, you will not generally be able to get to the exact ending point with different specified speeds, so I will show a solution, that gets just past the ending point and stops there.

import numpy as np

ue_speed = [3, 4, 8, 25]
ue_speed_prob = [0.4, 0.2, 0.3, 0.1]
square_size = 100 # need for scaling
time_interval = 100 # need for scaling
start_coord = [0, 0]
end_coord = [25, 15]
b = end_coord[0] - start_coord[0]
a = end_coord[1] - start_coord[1]
c = (a**2+b**2)**0.5

theta =  np.arctan2(a,b)

steps = int(c//(min(ue_speed)*time_interval/square_size) + 1) # fixed scaling issue
v = np.random.choice(ue_speed, size=steps, p=ue_speed_prob)
R = np.cumsum((v * time_interval) / square_size)
R = R[:np.argmax(R>c)+1]
P = np.column_stack((R*np.cos(theta)+start_coord[0],
                     R*np.sin(theta)+start_coord[1]))
trajectory = P

This will print trajectory as

[[ 2.57247878  1.54348727]
 [ 5.14495755  3.08697453]
 [12.00490096  7.20294058]
 [33.4422241  20.06533446]]

Note: You also made a very serious error in your code, when your converted the result of the math.asin to degrees and then used it again in trigonometric functions like np.sin and np.cos. I strongly recommand you to stick to radiant angle values and only convert them to degrees if you want to print them out.

Also I recommend the use of an arctan2 like function, to correctly get the angle of an x and y coordinate, as this will also work for negative x directions.

Python: How to create trajectory between start and end positions with specified speeds in a vectorized way?

Answers (2)

Related Questions