Source code for ktk.kinematics

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# Copyright 2020-2024 Félix Chénier

# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at

#     http://www.apache.org/licenses/LICENSE-2.0

# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
Provide functions related to kinematics analysis.
"""
__author__ = "Félix Chénier"
__copyright__ = "Copyright (C) 2020-2024 Félix Chénier"
__email__ = "chenier.felix@uqam.ca"
__license__ = "Apache 2.0"


import kineticstoolkit.geometry as geometry
from kineticstoolkit import TimeSeries
from kineticstoolkit.typing_ import check_param

import numpy as np
import warnings


def __dir__():
    return [
        "create_cluster",
        "extend_cluster",
        "track_cluster",
    ]


[docs] def create_cluster( markers: TimeSeries, /, names: list[str] ) -> dict[str, np.ndarray]: """ Create a cluster definition based on a static acquisition. Parameters ---------- markers Markers trajectories during a static acquisition. names The markers that define the cluster. Returns ------- dict dictionary where each entry represents the local position of a marker in an arbitrary coordinate system. Note ----- 0.10.0: Parameters `marker_names` was changed to `names` See Also -------- ktk.kinematics.extend_cluster ktk.kinematics.track_cluster """ check_param("markers", markers, TimeSeries) names = list(names) check_param("names", names, list, contents_type=str) n_samples = len(markers.time) n_markers = len(names) # Construct the global points array global_points = np.empty((n_samples, 4, n_markers)) for i_marker, marker in enumerate(names): global_points[:, :, i_marker] = markers.data[marker][:, :] # Remove samples where at least one marker is invisible global_points = global_points[~geometry.isnan(global_points)] rigid_bodies = geometry.create_frames( origin=global_points[:, :, 0], x=global_points[:, :, 1] - global_points[:, :, 0], xy=global_points[:, :, 2] - global_points[:, :, 0], ) local_points = geometry.get_local_coordinates(global_points, rigid_bodies) # Take the average local_points = np.array(np.mean(local_points, axis=0)) local_points = local_points[np.newaxis] # Create the output output = {} for i_marker, name in enumerate(names): output[name] = local_points[:, :, i_marker] return output
[docs] def extend_cluster( markers: TimeSeries, /, cluster: dict[str, np.ndarray], name: str ) -> dict[str, np.ndarray]: """ Add a point to an existing cluster. Parameters ---------- markers TimeSeries that includes the new point trajectory, along with point trajectories from the cluster definition. cluster The source cluster to add a new point to. name The name of the point to add (data key of the markers TimeSeries). Returns ------- dict[str, np.ndarray] A copy of the initial cluster, with the added point. Note ---- 0.10.0: Parameter `new_point` was changed to `name` See Also -------- ktk.kinematics.create_cluster ktk.kinematics.track_cluster """ check_param("markers", markers, TimeSeries) check_param("cluster", cluster, dict, key_type=str) for key in cluster: cluster[key] = np.array(cluster[key]) check_param("name", name, str) frames = _track_cluster_frames(markers, cluster) local_coordinates = geometry.get_local_coordinates( markers.data[name], frames ) if np.all(geometry.isnan(local_coordinates)): warnings.warn( f"The point {name} was invisible during the whole TimeSeries." ) else: cluster[name] = np.nanmean(local_coordinates, axis=0)[np.newaxis] return cluster
[docs] def track_cluster( markers: TimeSeries, /, cluster: dict[str, np.ndarray], *, include_lcs: bool = False, lcs_name: str = "LCS", ) -> TimeSeries: """ Fit a cluster to a TimeSeries of point trajectories. This function fits a cluster to a TimeSeries and reconstructs a solidified version of all the points defined in this cluster. Parameters ---------- markers A TimeSeries that contains point trajectories as Nx4 arrays. cluster A cluster definition as returned by ktk.kinematics.create_cluster(). include_lcs Optional. If True, return an additional entry in the output TimeSeries, that is the Nx4x4 frame series corresponding to the tracked cluster's local coordinate system. The default is False. lcs_name Optional. Name of the TimeSeries data entry for the tracked local coordinate system. The default is "LCS". Returns ------- TimeSeries A TimeSeries with the trajectories of all cluster points. See Also -------- ktk.kinematics.create_cluster ktk.kinematics.track_cluster """ check_param("markers", markers, TimeSeries) check_param("cluster", cluster, dict, key_type=str) for key in cluster: cluster[key] = np.array(cluster[key]) check_param("include_lcs", include_lcs, bool) check_param("lcs_name", lcs_name, str) out = markers.copy(copy_data=False, copy_data_info=False) unit = _get_marker_unit(markers) # Track the cluster frames = _track_cluster_frames(markers, cluster) for marker in cluster: out.data[marker] = geometry.get_global_coordinates( cluster[marker], frames ) if unit is not None: out.add_data_info(marker, "Unit", unit, in_place=True) if include_lcs: out.data[lcs_name] = frames return out
def _track_cluster_frames( markers: TimeSeries, cluster: dict[str, np.ndarray] ) -> np.ndarray: """Track a cluster and return its frame series.""" # Set local and global points marker_names = cluster.keys() stacked_local_points = np.dstack( [np.array(cluster[_]) for _ in marker_names] ) global_points = np.empty( (len(markers.time), 4, stacked_local_points.shape[2]) ) for i_marker, marker_name in enumerate(marker_names): if marker_name in markers.data: global_points[:, :, i_marker] = markers.data[marker_name] else: global_points[:, :, i_marker] = np.nan (stacked_local_points, global_points) = geometry._match_size( stacked_local_points, global_points ) # Track the cluster frames = geometry.register_points(global_points, stacked_local_points) return frames def _get_marker_unit(markers: TimeSeries) -> None | str: """Get markers unit, raise ValueError if not all have the same unit.""" unit = None for marker in markers.data: try: this_unit = markers.data_info[marker]["Unit"] except KeyError: this_unit = None if this_unit is not None: if unit is None: unit = this_unit else: if unit != this_unit: raise ValueError( "All markers must have the same unit. However, this " f"TimeSeries has both {unit} and {this_unit}." ) return unit def write_trc_file(markers: TimeSeries, /, filename: str) -> None: """ Export a markers TimeSeries to OpenSim's TRC file format. Parameters ---------- markers Markers trajectories. filename Name of the trc file to create. Warning ------- This function may eventually move either to the base namespace like write_c3d_file, or to an opensim extension that is currently being developed. """ check_param("markers", markers, TimeSeries) check_param("filename", filename, str) markers = markers.copy() markers.fill_missing_samples(0) n_markers = len(markers.data) n_frames = markers.time.shape[0] data_rate = n_frames / (markers.time[1] - markers.time[0]) camera_rate = data_rate units = "m" # Open file with open(filename, "w") as fid: fid.write(f"PathFileType\t4\t(X/Y/Z)\t{filename}\n") fid.write( "DataRate\tCameraRate\tNumFrames\tNumMarkers\tUnits\t" "OrigDataRate\tOrigDataStartFrame\tOrigNumFrames\n" ) fid.write( f"{data_rate}\t{camera_rate}\t{n_frames}\t{n_markers}\t" f"{units}\t{data_rate}\t1\t{n_frames}\n" ) # Write marker names fid.write("Frame#\tTime") for key in markers.data: fid.write(f"\t{key}\t\t") fid.write("\n") # Write coordinate names fid.write("\t") for i, key in enumerate(markers.data): fid.write(f"\tX{i+1}\tY{i+1}\tZ{i+1}") fid.write("\n\n") # Write trajectories for i_frame in range(n_frames): fid.write(f"{i_frame+1}\t" "{:.3f}".format(markers.time[i_frame])) for key in markers.data: fid.write( "\t{:.5f}".format(markers.data[key][i_frame, 0]) + "\t{:.5f}".format(markers.data[key][i_frame, 1]) + "\t{:.5f}".format(markers.data[key][i_frame, 2]) ) fid.write("\n")