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
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())
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()
# 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)