def calc_distance_between_points_in_a_vector_2d(x, y):
"""
Given a 2D array with eg X,Y tracking data it returns
the distance between each (x,y) point
"""
x_dot = np.abs(derivative(x))
y_dot = np.abs(derivative(y))
return np.sqrt(x_dot**2 + y_dot**2)
def from_tracking(
cls,
x: np.ndarray,
y: np.ndarray,
speed: np.ndarray,
theta: np.ndarray,
sample_every: int = 20,
):
accel = derivative(speed)
waypoints = []
for frame in np.arange(0, len(x), sample_every):
angle = theta[frame] + 90
if angle < 0:
angle += 360
elif angle > 360:
angle -= 360
waypoints.append(
Waypoint(
x[frame],
y[frame],
angle,
speed[frame],
accel[frame],
segment_duration=20 / 60,
))
return cls(waypoints=waypoints)
def compute_averaged_quantities(body_parts_tracking: dict) -> dict:
"""
For some things like orientation average across body parts to reduce noise
"""
# import matplotlib.pyplot as plt
def unwrap(x):
return np.degrees(np.unwrap(np.radians(x)))
# get data
body = pd.DataFrame(body_parts_tracking["body"]).interpolate(axis=0)
tail_base = pd.DataFrame(
body_parts_tracking["tail_base"]).interpolate(axis=0)
# get speed & acceleration
results = dict(speed=body.bp_speed.values.copy())
results["acceleration"] = derivative(results["speed"])
# get direction of movement
path = Path(body.x, body.y).smooth()
results["theta"] = path.theta
results["thetadot"] = path.thetadot # deg/s
results["thetadotdot"] = path.thetadotdot # in deg / s^2
# compute orientation of the body
results["orientation"] = orientation(tail_base.x, tail_base.y, body.x,
body.y)
# compute angular velocity in deg/s
results["angular_velocity"] = (angular_derivative(results["orientation"]) *
60) # in deg/s
return results
def ComputeControlCommand(self, target_speed, vehicle_state, dist):
"""
calc acceleration command using PID.
:param target_speed: target speed [m / s]
:param vehicle_state: vehicle state
:param dist: distance to goal [m]
:return: control command (acceleration) [m / s^2]
"""
error = target_speed - vehicle_state.speed
self.error_history.append(error)
a = 0
a += longitudinal_pid_P_gain * error
a += longitudinal_pid_D_gain * derivative(self.error_history)[-1]
if len(self.error_history) > 6:
a += longitudinal_pid_I_gain * np.sum(self.error_history[-5:])
if dist < 10.0:
if vehicle_state.speed > 2.0:
a = -3.0
elif vehicle_state.speed < -2:
a = -1.0
return a
def angular_derivative(angles: np.ndarray) -> np.ndarray:
"""
Takes the deriative of an angular variable (in degrees)
"""
# convert to radians and take derivative
rad = np.unwrap(np.deg2rad(angles))
diff = derivative(rad)
return np.rad2deg(diff)
def calc_angular_velocity(angles: np.ndarray) -> np.ndarray:
# convert to radians and take derivative
rad = np.unwrap(np.deg2rad(angles))
rad = medfilt(rad, 11)
rad = data_utils.convolve_with_gaussian(rad, 11)
diff = derivative(rad)
return np.rad2deg(diff)
def calc_ang_velocity(angles):
"""calc_ang_velocity [calculates the angular velocity ]
Arguments:
angles {[np.ndarray]} -- [1d array with a timeseries of angles in degrees]
Returns:
[np.ndarray] -- [1d array with the angular velocity in degrees at each timepoint]
testing:
>>> v = calc_ang_velocity([0, 10, 100, 50, 10, 0])
"""
# Check input data
if not isinstance(angles, np.ndarray) and not isinstance(angles, list):
raise ValueError("Invalid input data format")
if isinstance(angles, np.ndarray):
if len(angles.shape) > 1:
angles = angles.ravel()
# Calculate
angles_radis = np.unwrap(np.radians(np.nan_to_num(angles))) # <- to unwrap
ang_vel_rads = derivative(angles_radis)
return np.degrees(ang_vel_rads)
def calc_angle_between_points_of_vector_2d(x, y):
"""
Given 2 1d arrays specifying for instance the X and Y coordinates at each frame,
computes the angle between successive points (x,y)
"""
return np.degrees(np.arctan2(derivative(x), derivative(y)))
y = trk["y"][bout.start_frame : bout.end_frame].copy()
speed = trk["speed"][bout.start_frame : bout.end_frame].copy()
avel = trk["dmov_velocity"][bout.start_frame : bout.end_frame].copy()
dmov = trk["direction_of_movement"][
bout.start_frame : bout.end_frame
].copy()
coord = trk["global_coord"][bout.start_frame : bout.end_frame].copy()
avel[speed < 12] = np.nan
dmov[speed < 12] = np.nan
_data = dict(
x=x,
y=y,
speed=speed,
accel=derivative(speed),
avel=avel,
angacc=derivative(avel),
)
for k, v in _data.items():
data[k].extend(list(v))
# plot tracking
# plot speed and ang vel
# time = np.linspace(0, 1, len(speed))
# axes["B"].scatter(coord, speed, color=colors.speed, s=20, alpha=.1)
# axes["C"].scatter(coord, avel, color=colors.angular_velocity, s=20, alpha=.1)
# plt speed vs ang vel
# axes['E'].scatter(speed, abs(avel), color='k', alpha=.01)
def _compute_angular_vel_and_acc(self):
"""
Given theta, compute angular velocity and acceleration
"""
self.thetadot = angular_derivative(self.theta) * self.fps
self.thetadotdot = derivative(self.thetadot)
) = track.extract_track_from_image(
points_spacing=1,
restrict_extremities=False,
apply_extra_spacing=False,
)
tracking = Path(tracking_db.x[fast], tracking_db.y[fast])
linearized = TCS.path_to_track_coordinates_system(center_line, tracking)
# %%
from fcutils.maths import derivative
unit = units.iloc[0]
spikes = np.zeros_like(tracking_db.x)
spikes[unit.spikes] = 1
direction = derivative(tracking_db.global_coord)
direction[direction > 0] = 1
direction[direction < 0] = -1
data = pd.DataFrame(
dict(x=tracking_db.x,
y=tracking_db.y,
speed=tracking_db.speed,
spikes=spikes,
direction=direction))
data = data.loc[(data.speed > 20) & (data.spikes == 1) & (data.direction > 0)]
draw.Tracking.scatter(data.x.values, data.y.values)