def track_segment_with_custom_rt(penalty: PenaltyOption,
all_pn: PenaltyNodeList,
segment: Union[int, str], rt: int):
"""
Minimize the difference of the euler angles extracted from the coordinate system of a segment
and a RT (e.g. IMU). 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
segment: Union[int, str]
The name or index of the segment
rt: int
The index of the RT in the bioMod
"""
penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic
nlp = all_pn.nlp
segment_index = biorbd.segment_index(
nlp.model, segment) if isinstance(segment, str) else segment
r_seg = nlp.model.globalJCS(nlp.states["q"].mx,
segment_index).rot()
r_rt = nlp.model.RT(nlp.states["q"].mx, rt).rot()
angles_diff = biorbd.Rotation_toEulerAngles(
r_seg.transpose() * r_rt, "zyx").to_mx()
angle_objective = BiorbdInterface.mx_to_cx(f"track_segment",
angles_diff,
nlp.states["q"])
return angle_objective
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