custom_dynamics/root_implicit.py
def implicit_root_dynamics(all_pn: PenaltyNodeList, **unused_param):
    """
    Compute the difference between symbolic joint torques and inverse dynamic results
    It does not include any inversion of mass matrix

    Parameters
    ----------
    all_pn: PenaltyNodeList
        The penalty node elements
    **unused_param: dict
        Since the function does nothing, we can safely ignore any argument
    """

    nlp = all_pn.nlp
    nb_root = nlp.model.nbRoot()

    q = nlp.states["q"].mx
    qdot = nlp.states["qdot"].mx
    qddot = nlp.states["qddot"].mx if "qddot" in nlp.states.keys() else nlp.controls["qddot"].mx

    if nlp.external_forces:
        raise NotImplementedError(
            "This implicit constraint implicit_root_dynamics is not implemented yet with external forces"
        )

    floating_base_constraint = nlp.model.InverseDynamics(q, qdot, qddot).to_mx()[:nb_root]

    var = []
    var.extend([nlp.states[key] for key in nlp.states])
    var.extend([nlp.controls[key] for key in nlp.controls])
    var.extend([param for param in nlp.parameters])

    return BiorbdInterface.mx_to_cx("FloatingBaseConstraint", floating_base_constraint, *var)

    Compute the difference between symbolic joint torques and inverse dynamic results
    It does not include any inversion of mass matrix

    Parameters
    ----------
    all_pn: PenaltyNodeList
        The penalty node elements
    **unused_param: dict
        Since the function does nothing, we can safely ignore any argument
    
Compute the difference between symbolic joint torques and inverse dynamic results
It does not include any inversion of mass matrix

Parameters

PenaltyNodeList
The penalty node elements
unused_param: dict
Since the function does nothing, we can safely ignore any argument
Compute the difference between symbolic joint torques and inverse dynamic results It does not include any inversion of mass matrix Parameters PenaltyNodeList The penalty node elements unused_param : dict Since the function does nothing we can safely ignore any argument
def implicit_root_dynamics(all_pn: PenaltyNodeList, **unused_param):
    nlp = all_pn.nlp
    nb_root = nlp.model.nbRoot()
    q = nlp.states["q"].mx
    qdot = nlp.states["qdot"].mx
    qddot = nlp.states["qddot"].mx if "qddot" in nlp.states.keys() else nlp.controls["qddot"].mx
    if nlp.external_forces:
        raise NotImplementedError(
            "This implicit constraint implicit_root_dynamics is not implemented yet with external forces"
        )
    floating_base_constraint = nlp.model.InverseDynamics(q, qdot, qddot).to_mx()[:nb_root]
    var = []
    var.extend([nlp.states[key] for key in nlp.states])
    var.extend([nlp.controls[key] for key in nlp.controls])
    var.extend([param for param in nlp.parameters])
    return BiorbdInterface.mx_to_cx("FloatingBaseConstraint", floating_base_constraint, *var)
def implicit_root_dynamics ( all_pn : PenaltyNodeList , ** unused_param ) : nlp = all_pn . nlp nb_root = nlp . model . nbRoot ( ) q = nlp . states [ "q" ] . mx qdot = nlp . states [ "qdot" ] . mx qddot = nlp . states [ "qddot" ] . mx if "qddot" in nlp . states . keys ( ) else nlp . controls [ "qddot" ] . mx if nlp . external_forces : raise NotImplementedError ( "This implicit constraint implicit_root_dynamics is not implemented yet with external forces" ) floating_base_constraint = nlp . model . InverseDynamics ( q , qdot , qddot ) . to_mx ( ) [ : nb_root ] var = [ ] var . extend ( [ nlp . states [ key ] for key in nlp . states ] ) var . extend ( [ nlp . controls [ key ] for key in nlp . controls ] ) var . extend ( [ param for param in nlp . parameters ] ) return BiorbdInterface . mx_to_cx ( "FloatingBaseConstraint" , floating_base_constraint , * var )
Compute the difference between symbolic joint torques and inverse dynamic results
It does not include any inversion of mass matrix
Compute the difference between symbolic joint torques and inverse dynamic results It does not include any inversion of mass matrix
"""
    Compute the difference between symbolic joint torques and inverse dynamic results
    It does not include any inversion of mass matrix

    Parameters
    ----------
    all_pn: PenaltyNodeList
        The penalty node elements
    **unused_param: dict
        Since the function does nothing, we can safely ignore any argument
    """
all_pn
PenaltyNodeList