def custom_func_track_markers(pn: PenaltyNode, first_marker: str, second_marker: str) -> MX:
"""
The used-defined objective function (This particular one mimics the ObjectiveFcn.SUPERIMPOSE_MARKERS)
Except for the last two
Parameters
----------
pn: PenaltyNode
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
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
marker_0_idx = biorbd.marker_index(pn.nlp.model, first_marker)
marker_1_idx = biorbd.marker_index(pn.nlp.model, second_marker)
# Store the casadi function. Using add_casadi_func allow to skip if the function already exists
markers_func = pn.nlp.add_casadi_func("markers", pn.nlp.model.markers, pn.nlp.q)
# Get the marker positions and compute the difference
nq = pn.nlp.shape["q"]
q = pn.x[:nq]
markers = markers_func(q)
return markers[:, marker_0_idx] - markers[:, marker_1_idx]
def custom_func_track_markers(pn: PenaltyNode, first_marker: str,
second_marker: str) -> MX:
"""
The used-defined constraint function (This particular one mimics the ConstraintFcn.SUPERIMPOSE_MARKERS)
Except for the last two
Parameters
----------
pn: PenaltyNode
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
Returns
-------
The value that should be constrained in the MX format
"""
# Get the index of the markers
marker_0_idx = biorbd.marker_index(pn.nlp.model, first_marker)
marker_1_idx = biorbd.marker_index(pn.nlp.model, second_marker)
# Store the casadi function. Using add_casadi_func allow to skip if the function already exists
markers_func = pn.nlp.add_casadi_func("markers", pn.nlp.model.markers,
pn.nlp.states["q"].mx)
# Get the marker positions and compute the difference
markers = markers_func(pn["q"])
return markers[:, marker_0_idx] - markers[:, marker_1_idx]
def test_name_to_index():
m = biorbd.Model("../../models/pyomecaman.bioMod")
# Index of a segment
np.testing.assert_equal(biorbd.segment_index(m, "Pelvis"), 0)
np.testing.assert_equal(biorbd.segment_index(m, "PiedG"), 10)
with pytest.raises(ValueError, match="dummy is not in the biorbd model"):
biorbd.segment_index(m, "dummy")
# Index of a marker
np.testing.assert_equal(biorbd.marker_index(m, "pelv1"), 0)
np.testing.assert_equal(biorbd.marker_index(m, "piedg6"), 96)
with pytest.raises(ValueError, match="dummy is not in the biorbd model"):
biorbd.marker_index(m, "dummy")
def custom_func_track_markers(pn: PenaltyNode, first_marker: str,
second_marker: str) -> MX:
# Get the index of the markers
marker_0_idx = biorbd.marker_index(pn.nlp.model, first_marker)
marker_1_idx = biorbd.marker_index(pn.nlp.model, second_marker)
# Store the casadi function. Using add_casadi_func allow to skip if the function already exists
markers_func = pn.nlp.add_casadi_func("markers", pn.nlp.model.markers,
pn.nlp.q)
# Get the marker positions and compute the difference
nq = pn.nlp.shape["q"]
q = pn.x[:nq]
markers = markers_func(q)
return markers[:, marker_0_idx] - markers[:, marker_1_idx]