def robot_curve(self, curve_type: CurveType, side: RobotSide):
"""
Calculates the given curve for the given side of the robot.
:param curve_type: The type of the curve to calculate
:param side: The side to use in the calculation
:return: The points of the calculated curve
"""
coeff = (self.robot.robot_info[3] /
2) * (1 if side == RobotSide.LEFT else -1)
cp = self.control_points()
t = linspace(0, 1, samples=Trajectory.SAMPLE_SIZE + 1)
curves = [
Curve(control_points=points,
spline_type=SplineType.QUINTIC_HERMITE) for points in cp
]
dx, dy = npconcat([c.calculate(t, CurveType.VELOCITY)
for c in curves]).T
theta = nprads(angle_from_slope(dx, dy))
points = npconcat([c.calculate(t, curve_type) for c in curves])
normals = coeff * nparray([-npsin(theta), npcos(theta)]).T
return points + normals
def distance(self, concat: bool = True):
"""
Calculates the distance passed by the middle of the robot through the curve.
:return: The distance passed through the curve
"""
cp = self.control_points()
curves = [
Curve(control_points=points,
spline_type=SplineType.QUINTIC_HERMITE) for points in cp
]
seg_lengths = [
length_integral(
0, 1, lambda u: curves[i].calculate(u, CurveType.VELOCITY),
Trajectory.L_SAMPLE_SIZE) for i in range(self.num_of_segments)
]
sums = [
sum([seg_lengths[j] for j in range(i)]) if i > 0 else 0
for i in range(self.num_of_segments)
]
lengths = [[[
i + j / Trajectory.SAMPLE_SIZE,
length_integral(
0, j / Trajectory.SAMPLE_SIZE,
lambda u: curves[i].calculate(u, CurveType.VELOCITY),
Trajectory.L_SAMPLE_SIZE) + sums[i]
] for j in range(Trajectory.SAMPLE_SIZE + 1)]
for i in range(self.num_of_segments)]
return npconcat(lengths) if concat else lengths
def evolve(self):
"""Envove one time and update the group."""
newbirth_n = int(np.round(self.group.size * self.group.birth_rate))
keepalive_n = int(np.round(self.group.size * self.group.keep_rate))
pair_num = self.genome.get_pair_num(newbirth_n)
cross_pairs = _get_cross_pairs(self.fitness_scores, pair_num,
self.group.matepool_size)
descendants = self._mutate(self._crossover(cross_pairs))
keepmembers, keepscores = self._best_individuals_select(keepalive_n)
descendant_scores = np.array(
self.fitness_eval(descendants, self.environment))
self.fitness_scores = npconcat((descendant_scores, keepscores))
self.group.members = npconcat((descendants, keepmembers))
self.group.size = newbirth_n + keepalive_n
self.genome.mutate_decay()
def headings(self):
"""
Calculates the robot's heading angles for each point in the curve (theta(t)) and calculates the angular velocity
in each point on the curve (theta'(t)).
:return: A tuple consisting of the values of theta(t) and theta'(t) through the curve.
"""
cp = self.control_points()
t = linspace(0, 1, samples=Trajectory.SAMPLE_SIZE + 1)
curves = [
Curve(control_points=points,
spline_type=SplineType.QUINTIC_HERMITE) for points in cp
]
dx, dy = npconcat([c.calculate(t, CurveType.VELOCITY)
for c in curves]).T
d2x, d2y = npconcat(
[c.calculate(t, CurveType.ACCELERATION) for c in curves]).T
return angle_from_slope(dx,
dy), ((d2y * dx - d2x * dy) / (dx**2 + dy**2))
def speed(self, concat: bool = True):
"""
Calcualtes the speed of the _middle_ of the robot.
:return: A numpy list of vectors [t, s(t)] for the time and speed values
"""
velocities = self.curve(CurveType.VELOCITY, concat=False)
times = [w.time for w in self.waypoints]
speeds = [[[
(times[i + 1] - times[i]) * (j / Trajectory.SAMPLE_SIZE) +
times[i],
sqrt(velocities[i][j][0]**2 + velocities[i][j][1]**2),
] for j in range(Trajectory.SAMPLE_SIZE + 1)]
for i in range(self.num_of_segments)]
return npconcat(speeds) if concat else speeds
def curve(self, curve_type: CurveType, concat: bool = True):
"""
Calculates the curve corresponding to the given type for the _middle_ of the robot.
:param curve_type: The curve type wanted to calculate
:param concat: Should concat the segments or not. Default - false
:return: A list of numpy point vectors if concat is false. Else - one huge numpy vector
"""
cp = self.control_points()
t = linspace(0, 1, samples=Trajectory.SAMPLE_SIZE + 1)
curves = [
Curve(control_points=points,
spline_type=SplineType.QUINTIC_HERMITE).calculate(
t, curve_type) for points in cp
]
return npconcat(curves) if concat else curves
def concatenate(*args, **kwargs):
return from_array(npconcat(*args))
def render(self):
print('Position:')
print(self.format(self.trajectory.curve(CurveType.POSITION)))
print('Velocity vectors:')
print(self.format(self.trajectory.curve(CurveType.VELOCITY)))
midvel = self.trajectory.speed(concat=False)
print('Velocity:')
print(self.format(npconcat(midvel)))
print('Left position:')
print(
self.format(
self.trajectory.robot_curve(CurveType.POSITION,
RobotSide.LEFT)))
print('Right position:')
print(
self.format(
self.trajectory.robot_curve(CurveType.POSITION,
RobotSide.RIGHT)))
free_speed = self.trajectory.robot.chassis_info[0]
vleft = self.trajectory.robot_speeds(RobotSide.LEFT)
vright = self.trajectory.robot_speeds(RobotSide.RIGHT)
print('Left velocity:')
print(self.format(vleft))
print('Right velocity:')
print(self.format(vright))
print('Left output:')
print(self.format(nparray([[v[0], v[1] / free_speed] for v in vleft])))
print('Right output:')
print(self.format(nparray([[v[0], v[1] / free_speed]
for v in vright])))
snc = self.trajectory.distance(concat=False)
print('Middle distances:')
print(self.format(npconcat(snc)))
sp = [
clamp_to_bounds(-(0.9 * free_speed), 0.9 * free_speed,
max([v[1] for v in seg])) for seg in midvel
]
print([
self.trajectory.robot.dist_to_vel(v, sp[i - 1] if i > 0 else 0)
for (i, v) in enumerate(sp)
])
print('Middle distances vs max speeds:')
print(
self.format(
npconcat([
[
[
# v[1],
sp[i]
] for v in seg
] for (i, seg) in enumerate(snc)
])))
concat as pdconcat
from numpy import arange, random,\
concatenate as npconcat
# Create numpy arrays
a1 = random.randn(25).reshape(5, 5)
b1 = arange(25).reshape(5, 5)
print '--- a1 ---'
print a1
print '--- b1 ---'
print b1
# Concatenate/link numpy arrays
print '--- Concatenate a1, b1 rows(axis=0)'
print npconcat([a1, b1], axis=0)
print '--- Concatenate a1, b1 columns(axis=1)'
print npconcat([a1, b1], axis=1)
# Create Series
s1 = Series([100, 200, 300], index=['A', 'B', 'C'])
s2 = Series([400, 500], index=['D', 'E'])
print '--- s1 ---'
print s1
print '--- s2 ---'
print s2
# Concatenate/link Series
print '--- concat(s1,s2) axis = 0---'
print pdconcat([s1, s2])
print '--- concat(s2, s1) axis = 0---'