到目前为止,我有以下答案:
from copy import copy
import numpy as np
class ZigZagTraj(object):
def __init__(self, xs, ys, xe, ye, tooth_direction='x', tooth_width=0.2, step=0.05):
self.xs, self.ys, self.xe, self.ye = xs, ys, xe, ye
self.tooth_direction = tooth_direction
self.tooth_width = tooth_width
self.step = step
self.bounds = {'x': {'lb':None, 'ub':None },
'y': {'lb':None, 'ub':None }}
if xe - xs > 0:
self.bounds['x']['ub'] = self.xe
self.bounds['x']['lb'] = self.xs
else:
self.bounds['x']['ub'] = self.xs
self.bounds['x']['lb'] = self.xe
if ye - ys > 0:
self.bounds['y']['ub'] = self.ye
self.bounds['y']['lb'] = self.ys
else:
self.bounds['y']['ub'] = self.ys
self.bounds['y']['lb'] = self.ye
self.x_increment_sign = np.sign(self.xe - self.xs)
self.y_increment_sign = np.sign(self.ye - self.ys)
self.curr_direction = tooth_direction
self.curr_tooth_width = 0
self.traj = []
self.traj.append([xs, ys])
def change_curr_direction(self):
if self.curr_direction == 'x':
self.curr_direction = 'y'
elif self.curr_direction == 'y':
self.curr_direction = 'x'
return self.curr_direction
def in_bound(self,new_point):
return self.in_x_bound(new_point) and self.in_y_bound(new_point)
def in_x_bound(self,new_point):
return self.bounds['x']['lb'] <= new_point[0] <= self.bounds['x']['ub']
def in_y_bound(self, new_point):
return self.bounds['y']['lb'] <= new_point[1] <= self.bounds['y']['ub']
def generate_trajectory(self):
self.tooth_direction = self.tooth_direction
curr_tooth_width = 0
traj = []
traj.append([self.xs, self.ys])
#print('self.x_increment_sign',self.x_increment_sign, 'self.x_increment_sign',self.x_increment_sign)
while True:
#print(traj[-1], 'for direction', self.curr_direction)
new_point = copy(traj[-1])
if self.curr_direction == 'x':
new_point[0] += self.step * self.x_increment_sign
elif self.curr_direction == 'y':
new_point[1] += self.step * self.y_increment_sign
if self.curr_direction != self.tooth_direction: # we're on a tooth
curr_tooth_width += self.step
#print('checking if tooth width is reached', curr_tooth_width, tooth_width)
if curr_tooth_width >= self.tooth_width:
if self.tooth_direction == 'x': # along the tooth direction we should now run backwards
self.x_increment_sign *= -1
else:
self.y_increment_sign *= -1
curr_tooth_width = 0
self.change_curr_direction()
#print('not append(tooth width). moving to direction: ', self.curr_direction)
continue
elif not self.in_bound(new_point): # we're on tooth length ( out of bound for lenght direction)
self.change_curr_direction()
#print('not append(tooth length)')
continue
#print('checking if we should append')
#print('new point', new_point)
#print(self.in_x_bound(new_point), self.in_y_bound(new_point))
if not self.in_x_bound(new_point) or not self.in_y_bound(new_point):
#print('not append(break)')
break
new_point[0] = round(new_point[0], 3)
new_point[1] = round(new_point[1], 3)
traj.append(new_point)
return np.array(traj)
结果如下:
[![zig = ZigZagTraj( 1.2, 1.2, 0, 0, tooth_direction='x', tooth_width=0.2, step=0.05)
traj = zig.generate_trajectory()
fig, ax = plt.subplots()
ax.scatter(traj\[:, 0\], traj\[:, 1\])
plt.show()
