Exemplo n.º 1
0
    def __compute_all_values(self):
        q_idx = self.dof_mapping[self.current_dof]
        x_axis, all_q = self.__generate_x_axis(q_idx)
        length = np.ndarray((self.n_point_for_q, self.n_mus))
        moment_arm = np.ndarray((self.n_point_for_q, self.n_mus))
        passive_forces = np.ndarray((self.n_point_for_q, self.n_mus))
        active_forces = np.ndarray((self.n_point_for_q, self.n_mus))
        emg = biorbd.StateDynamics(0, self.active_forces_slider.value() / 100)
        for i, q_mod in enumerate(all_q):
            self.model.UpdateKinematicsCustom(biorbd.GeneralizedCoordinates(q_mod))
            muscles_length_jacobian = self.model.musclesLengthJacobian().to_array()
            for m in range(self.n_mus):
                if self.checkboxes_muscle[m].isChecked():
                    mus_group_idx, mus_idx, _ = self.muscle_mapping[self.checkboxes_muscle[m].text()]
                    mus = self.model.muscleGroup(mus_group_idx).muscle(mus_idx)
                    mus.updateOrientations(self.model, q_mod, 1)

                    length[i, m] = mus.length(self.model, q_mod, False)
                    moment_arm[i, m] = muscles_length_jacobian[mus_idx, q_idx]
                    if mus.type() != biorbd.IDEALIZED_ACTUATOR:
                        passive_forces[i, m] = biorbd.HillType(mus).FlPE()
                    else:
                        passive_forces[i, m] = 0
                    if mus.type() != biorbd.IDEALIZED_ACTUATOR:
                        active_forces[i, m] = biorbd.HillType(mus).FlCE(emg)
                    else:
                        active_forces[i, m] = 0

        return x_axis, length, moment_arm, passive_forces, active_forces
def objective_function(x, *args, **kwargs):
    # Update the model
    Q = biorbd.GeneralizedCoordinates(np.array(x))
    m.UpdateKinematicsCustom(Q)
    if show_live_optim:
        b.set_q(Q.to_array())
        b.refresh_window()

    # Get the tag to match
    bow = m.marker(Q, tabBowPosition, True, False).to_array()
    string = m.marker(Q, tagViolin, True, False).to_array()
    bow_position_on_violin = bow - string

    # Get the bow to align with the surrounding strings
    bow_frog = m.marker(Q, tagBowFrog, True, False).to_array()
    bow_tip = m.marker(Q, tagBowTip, True, False).to_array()
    string_behind = m.marker(Q, tagStringsAround[1], True, False).to_array()
    string_front = m.marker(Q, tagStringsAround[0], True, False).to_array()
    bow = bow_tip - bow_frog
    bow = bow / np.linalg.norm(bow)
    bow_expected = string_behind - string_front
    bow_expected = bow_expected / np.linalg.norm(bow_expected)
    bow_direction = bow_expected.dot(bow)

    # horsehair on the string
    bow_z_axis = m.globalJCS(idxSegmentBow).to_array()[0:3, 2]
    violin_z_axis = -m.globalJCS(idxSegmentViolin).to_array()[0:3, 2]
    z_axes_alignment = violin_z_axis.dot(bow_z_axis)

    out = np.ndarray((5, ))
    out[:3] = bow_position_on_violin
    out[3] = (1 - bow_direction) * 100
    out[4] = 1 - z_axes_alignment
    return out
Exemplo n.º 3
0
def test_np_mx_to_generalized():
    biorbd_model = biorbd.Model("../../models/pyomecaman.bioMod")

    q = biorbd.GeneralizedCoordinates(biorbd_model)
    qdot = biorbd.GeneralizedVelocity((biorbd_model.nbQdot()))
    qddot = biorbd.GeneralizedAcceleration((biorbd_model.nbQddot()))
    tau = biorbd_model.InverseDynamics(q, qdot, qddot)
    biorbd_model.ForwardDynamics(q, qdot, tau)

    if biorbd.currentLinearAlgebraBackend() == 1:
        tau = biorbd_model.InverseDynamics(q.to_mx(), qdot.to_mx(),
                                           qddot.to_mx())
        biorbd_model.ForwardDynamics(q, qdot, tau.to_mx())
    else:
        tau = biorbd_model.InverseDynamics(q.to_array(), qdot.to_array(),
                                           qddot.to_array())
        biorbd_model.ForwardDynamics(q, qdot, tau.to_array())
Exemplo n.º 4
0
    def set_q(self, Q, refresh_window=True):
        """
        Manually update
        Args:
            Q: np.array
                Generalized coordinate
            refresh_window: bool
                If the window should be refreshed now or not
        """
        if not isinstance(
                Q, np.ndarray) and len(Q.shape) > 1 and Q.shape[0] != self.nQ:
            raise TypeError(f"Q should be a {self.nQ} column vector")
        self.Q = Q

        self.model.UpdateKinematicsCustom(biorbd.GeneralizedCoordinates(
            self.Q))
        if self.show_muscles:
            self.__set_muscles_from_q()
        if self.show_local_ref_frame:
            self.__set_rt_from_q()
        if self.show_meshes:
            self.__set_meshes_from_q()
        if self.show_global_center_of_mass:
            self.__set_global_center_of_mass_from_q()
        if self.show_segments_center_of_mass:
            self.__set_segments_center_of_mass_from_q()
        if self.show_markers:
            self.__set_markers_from_q()

        # Update the sliders
        if self.show_analyses_panel:
            for i, slide in enumerate(self.sliders):
                slide[1].blockSignals(True)
                slide[1].setValue(self.Q[i] * self.double_factor)
                slide[1].blockSignals(False)
                slide[2].setText(f"{self.Q[i]:.2f}")

        if refresh_window:
            self.refresh_window()
Exemplo n.º 5
0
# c3d = ezc3d.c3d(data_filename)
# markers = c3d['data']['points'][:3, :, :]  # XYZ1 x markers x time_frame

# Dispatch markers in biorbd structure so EKF can use it
markersOverFrames = []
for i in range(markers.shape[2]):
    markersOverFrames.append(
        [biorbd.NodeSegment(m) for m in markers[:, :, i].T])

# Create a Kalman filter structure
freq = 100  # Hz
params = biorbd.KalmanParam(freq)
kalman = biorbd.KalmanReconsMarkers(model, params)

# Perform the kalman filter for each frame (the first frame is much longer than the next)
Q = biorbd.GeneralizedCoordinates(model)
Qdot = biorbd.GeneralizedVelocity(model)
Qddot = biorbd.GeneralizedAcceleration(model)
q_recons = np.ndarray((model.nbQ(), len(markersOverFrames)))
for i, targetMarkers in enumerate(markersOverFrames):
    kalman.reconstructFrame(model, targetMarkers, Q, Qdot, Qddot)
    q_recons[:, i] = Q.to_array()

    # Print the kinematics to the console
    print(
        f"Frame {i}\nExpected Q = {target_q[:, i]}\nCurrent Q = {q_recons[:, i]}\n"
    )

# Animate the results if biorbd viz is installed
if biorbd_viz_found:
    b = BiorbdViz.BiorbdViz(loaded_model=model)