def custom_func_track_markers(all_pn: PenaltyNodeList, first_marker: str,
second_marker: str) -> MX:
# Get the index of the markers from their name
marker_0_idx = biorbd.marker_index(all_pn.nlp.model, first_marker)
marker_1_idx = biorbd.marker_index(all_pn.nlp.model, second_marker)
# Convert the function to the required format and then subtract
markers = BiorbdInterface.mx_to_cx("markers", all_pn.nlp.model.markers,
all_pn.nlp.states["q"])
return markers[:, marker_1_idx] - markers[:, marker_0_idx]
def set_idx_columns(penalty: PenaltyOption, all_pn: PenaltyNodeList,
index: Union[str, int, list, tuple], _type: str):
"""
Simple penalty.cols setter for marker index and names
Parameters
----------
penalty: PenaltyOption
The actual penalty to declare
all_pn: PenaltyNodeList
The penalty node elements
index: Union[str, int, list, tuple]
The marker to index
_type: str
The type of penalty (for raise error message purpose)
"""
if penalty.cols is not None and index is not None:
raise ValueError(
f"It is not possible to define cols and {_type}_index since they are the same variable"
)
penalty.cols = index if index is not None else penalty.cols
if penalty.cols is not None:
penalty.cols = [
penalty.cols
] if not isinstance(penalty.cols, (tuple, list)) else penalty.cols
# Convert to int if it is str
if _type == "marker":
penalty.cols = [
cols if isinstance(cols, int) else biorbd.marker_index(
all_pn.nlp.model, cols) for cols in penalty.cols
]
def superimpose_markers(
penalty: PenaltyOption,
all_pn: PenaltyNodeList,
first_marker: Union[str, int],
second_marker: Union[str, int],
axes: Union[tuple, list] = None,
):
"""
Minimize the distance between two markers
By default this function is quadratic, meaning that it minimizes distance between them.
Parameters
----------
penalty: PenaltyOption
The actual penalty to declare
all_pn: PenaltyNodeList
The penalty node elements
first_marker: Union[str, int]
The name or index of one of the two markers
second_marker: Union[str, int]
The name or index of one of the two markers
axes: Union[tuple, list]
The axes to project on. Default is all axes
"""
nlp = all_pn.nlp
first_marker_idx = (biorbd.marker_index(nlp.model, first_marker)
if isinstance(first_marker, str) else
first_marker)
second_marker_idx = (biorbd.marker_index(nlp.model, second_marker)
if isinstance(second_marker, str) else
second_marker)
PenaltyFunctionAbstract._check_idx(
"marker", [first_marker_idx, second_marker_idx],
nlp.model.nbMarkers())
PenaltyFunctionAbstract.set_axes_rows(penalty, axes)
penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic
marker_0 = BiorbdInterface.mx_to_cx(f"markers_{first_marker}",
nlp.model.marker,
nlp.states["q"],
first_marker_idx)
marker_1 = BiorbdInterface.mx_to_cx(f"markers_{second_marker}",
nlp.model.marker,
nlp.states["q"],
second_marker_idx)
return marker_1 - marker_0
def custom_func_track_markers(all_pn: PenaltyNodeList, first_marker: str,
second_marker: str, method: int) -> MX:
"""
The used-defined objective function (This particular one mimics the ObjectiveFcn.SUPERIMPOSE_MARKERS)
Except for the last two
Parameters
----------
all_pn: PenaltyNodeList
The penalty node elements
first_marker: str
The index of the first marker in the bioMod
second_marker: str
The index of the second marker in the bioMod
method: int
Two identical ways are shown to help the new user to navigate the biorbd API
Returns
-------
The cost that should be minimize in the MX format. If the cost is quadratic, do not put
the square here, but use the quadratic=True parameter instead
"""
# Get the index of the markers from their name
marker_0_idx = biorbd.marker_index(all_pn.nlp.model, first_marker)
marker_1_idx = biorbd.marker_index(all_pn.nlp.model, second_marker)
if method == 0:
# Convert the function to the required format and then subtract
markers = BiorbdInterface.mx_to_cx("markers", all_pn.nlp.model.markers,
all_pn.nlp.states["q"])
markers_diff = markers[:, marker_1_idx] - markers[:, marker_0_idx]
else:
# Do the calculation in biorbd API and then convert to the required format
markers = all_pn.nlp.model.markers(all_pn.nlp.states["q"].mx)
markers_diff = markers[marker_1_idx].to_mx(
) - markers[marker_0_idx].to_mx()
markers_diff = BiorbdInterface.mx_to_cx("markers", markers_diff,
all_pn.nlp.states["q"])
return markers_diff
def track_marker_with_segment_axis(
penalty: PenaltyOption,
all_pn: PenaltyNodeList,
marker: Union[int, str],
segment: Union[int, str],
axis: Axis,
):
"""
Track a marker using a segment, that is aligning an axis toward the marker
By default this function is quadratic, meaning that it minimizes the difference.
Parameters
----------
penalty: PenaltyOption
The actual penalty to declare
all_pn: PenaltyNodeList
The penalty node elements
marker: int
Name or index of the marker to be tracked
segment: int
Name or index of the segment to align with the marker
axis: Axis
The axis that should be tracking the marker
"""
if not isinstance(axis, Axis):
raise RuntimeError("axis must be a bioptim.Axis")
penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic
nlp = all_pn.nlp
marker_idx = biorbd.marker_index(nlp.model, marker) if isinstance(
marker, str) else marker
segment_idx = biorbd.segment_index(
nlp.model, segment) if isinstance(segment, str) else segment
# Get the marker in rt reference frame
jcs = nlp.model.globalJCS(nlp.states["q"].mx, segment_idx)
marker = nlp.model.marker(nlp.states["q"].mx, marker_idx)
marker.applyRT(jcs.transpose())
marker_objective = BiorbdInterface.mx_to_cx(
"marker", marker.to_mx(), nlp.states["q"])
# To align an axis, the other must be equal to 0
if penalty.rows is not None:
raise ValueError(
"rows cannot be defined in track_marker_with_segment_axis")
penalty.rows = [
ax for ax in [Axis.X, Axis.Y, Axis.Z] if ax != axis
]
return marker_objective
