def __sub__(self, other):
result = PoseR2S2()
result.x = (self.x - other.x) * cos(other.theta) + (self.y - other.y) * sin(other.theta)
result.y = -(self.x - other.x) * sin(other.theta) + (self.y - other.y) * cos(other.theta)
result.theta = helper.wrap_to_npi_pi(self.theta - other.theta)
result.phi = self.phi
return result
def __add__(self, other):
result = PoseR2S2()
result.x = self.x + other.x * cos(self.theta) - other.y * sin(self.theta)
result.y = self.y + other.x * sin(self.theta) + other.y * cos(self.theta)
result.theta = helper.wrap_to_npi_pi(self.theta + other.theta)
result.phi = other.phi
return result
def execute_motion(self, pose: PoseR2S2, v: float, w: float,
dt: float) -> PoseR2S2:
theta1 = pose.theta
theta2 = wrap_to_npi_pi(theta1 - pose.phi)
# tan_alpha = w * self.tractor_l / v
L2 = self.trailer_l + self.link_l
x_new = pose.x + dt * v * cos(pose.phi) * cos(theta2)
y_new = pose.y + dt * v * cos(pose.phi) * sin(theta2)
theta_new = theta1 + dt * w
phi_new = theta_new - (theta2 + dt * v * sin(pose.phi) / L2)
return PoseR2S2(x_new, y_new, theta_new, phi_new)
def build_cpoints(self):
from math import exp
r = self.vehicle.tractor_l # see ref 1
# generate trajectories for each steering angle
for alpha in np.arange(-self.alpha_max,
self.alpha_max + self.alpha_resolution,
self.alpha_resolution):
n = 0
v = self.K * self.vehicle.v_max
pose = PoseR2S2(0., 0., 0., self.init_phi)
dist = 0. # tp_space distance
last_pose = pose.copy()
w = v / self.vehicle.tractor_l * tan(alpha) # rotational velocity
rotation = 0. # same as pose.theta but defined over the range [0: 2PI)
self.idx_to_alpha.append(alpha)
cpoints_at_alpha = [CPoint(pose, 0., v, w,
alpha)] # type: List[CPoint]
while abs(
rotation
) < 1.95 * PI and dist < self.d_max and n < self.n_max and abs(
pose.phi) <= self.vehicle.phi_max:
# calculate control parameters
delta = helper.wrap_to_npi_pi(alpha - pose.theta)
v = self.K * self.vehicle.v_max * exp(-(delta / 1.)**2)
w = self.K * self.vehicle.w_max * (-0.5 +
(1 /
(1 + exp(-delta / 1.))))
pose = self.vehicle.execute_motion(pose, v, w, self.dt)
rotation += w * self.dt
v_tp_space = sqrt(v * v + (w * r) * (w * r))
dist += v_tp_space * self.dt
delta_pose = pose - last_pose
dist1 = delta_pose.norm
dist2 = abs(delta_pose.theta) * self.k_theta
dist_max = max(dist1, dist2)
if dist_max > self.min_dist_between_cpoints:
cpoints_at_alpha.append(
CPoint(pose.copy(), dist, v, w, alpha, n))
last_pose.copy_from(pose)
n += 1
self.cpoints.append(cpoints_at_alpha)
print('Completed building cpoints for {0}'.format(self.name))
def alpha2idx(self, alpha: float) -> int:
alpha = helper.wrap_to_npi_pi(alpha)
if abs(alpha) > self.alpha_max:
alpha = np.sign(alpha) * self.alpha_max
delta = alpha + self.alpha_max
return int(np.rint(delta / self.alpha_resolution))
def diff(self, other):
result = PoseR2S2(self.x - other.x, self.y - other.y, helper.wrap_to_npi_pi(self.theta - other.theta), self.phi)
return result
def execute_motion(self, pose: PoseR2S2, v: float, w: float,
dt: float) -> PoseR2S2:
x_new = pose.x + dt * v * cos(pose.theta)
y_new = pose.y + dt * v * sin(pose.theta)
theta_new = wrap_to_npi_pi(pose.theta + dt * w)
return PoseR2S2(x_new, y_new, theta_new)