def align_segment_with_custom_rt(penalty_type, ocp, nlp, t, x, u, segment_idx, rt_idx, **extra_param):
"""
Adds the constraint that the RT and the segment must be aligned at the desired instant(s).
:param nlp: An OptimalControlProgram class.
:param X: List of instant(s).
:param policy: Tuple of indices of segment and rt.
"""
PenaltyFunctionAbstract._check_idx("segment", segment_idx, nlp["model"].nbSegment())
PenaltyFunctionAbstract._check_idx("rt", rt_idx, nlp["model"].nbRTs())
nq = nlp["q_mapping"].reduce.len
for v in x:
q = nlp["q_mapping"].expand.map(v[:nq])
r_seg = nlp["model"].globalJCS(q, segment_idx).rot()
r_rt = nlp["model"].RT(q, rt_idx).rot()
val = biorbd.Rotation_toEulerAngles(r_seg.transpose() * r_rt, "zyx").to_mx()
penalty_type._add_to_penalty(ocp, nlp, val, **extra_param)
def biorbd_meta_func(q, segment_idx, rt_idx):
r_seg = nlp.model.globalJCS(q, segment_idx).rot()
r_rt = nlp.model.RT(q, rt_idx).rot()
return biorbd.Rotation_toEulerAngles(r_seg.transpose() * r_rt,
"zyx").to_mx()