def dynamics_with_contact(t_int, states, biorbd_model, u, force_no_muscle=False): nb_q = biorbd_model.nbQ() nb_tau = biorbd_model.nbGeneralizedTorque() nb_muscle = biorbd_model.nbMuscleTotal() q = states[:nb_q] qdot = states[nb_q:] if nb_muscle > 0 and not force_no_muscle: states_dynamics = biorbd.VecBiorbdMuscleStateDynamics(nb_muscle) for i in range(len(states_dynamics)): states_dynamics[i].setActivation(u[i]) tau = biorbd_model.muscularJointTorque(states_dynamics, q, qdot).to_array() else: tau = np.zeros(nb_tau) tau += u[nb_muscle:nb_muscle + nb_tau] cs = biorbd_model.getConstraints() qddot = biorbd.Model.ForwardDynamicsConstraintsDirect( biorbd_model, states[:nb_q], states[nb_q:], tau, cs).to_array() return np.concatenate((qdot, qddot))
def forward_dynamics_muscle_excitations_and_torque_driven(states, controls, nlp): q, qdot, residual_tau = Dynamics.__dispatch_data(states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleStateDynamics(nlp["nbMuscle"]) muscles_excitation = controls[nlp["nbTau"] :] muscles_activations = states[nlp["nbQ"] + nlp["nbQdot"] :] # muscles_activations_dot = [] muscles_activations_dot = np.zeros(nlp["nbMuscle"]) # muscles_activations_dot = MX.sym("act_dot", nlp["nbMuscle"], 1) for k in range(nlp["nbMuscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) muscles_states[k].setActivation(muscles_activations[k]) # muscles_activations_dot.append(muscles_states[k].timeDerivativeActivation().to_mx()) # muscles_activations_dot[k] = muscles_states[k].timeDerivativeActivation().to_mx() # muscles_activations_dot[k] = muscles_states[k].timeDerivativeActivation(muscles_excitation[k], muscles_activations[k]).to_mx() # muscles_activations_dot = np.append(muscles_activations_dot, muscles_states[k].timeDerivativeActivation().to_mx) muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx() muscles_activations_dot = nlp["model"].activationDot(muscles_states).to_mx() # muscles_activations_dot = muscles_states.timeDerivativeActivation(muscles_excitation, muscles_activations) # muscles_activations_dot = nlp["model"].timeDerivativeActivation(muscles_activations, muscles_excitation) tau = muscles_tau + residual_tau qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(nlp["model"], q, qdot, tau).to_mx() qdot_reduced = nlp["q_mapping"].reduce.map(qdot) qddot_reduced = nlp["q_dot_mapping"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forward_dynamics_torque_muscle_driven_with_contact(states, controls, nlp): q, qdot, residual_tau = Dynamics.__dispatch_data(states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleStateDynamics(nlp["nbMuscle"]) muscles_activations = controls[nlp["nbTau"] :] for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(muscles_activations[k]) muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx() tau = muscles_tau + residual_tau qddot = biorbd.Model.ForwardDynamicsConstraintsDirect(nlp["model"], q, qdot, tau).to_mx() qdot_reduced = nlp["q_mapping"].reduce.map(qdot) qddot_reduced = nlp["q_dot_mapping"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced)
def forward_dynamics_muscle_activations_driven(states, controls, nlp): nq = nlp["q_mapping"].reduce.len q = nlp["q_mapping"].expand.map(states[:nq]) qdot = nlp["q_dot_mapping"].expand.map(states[nq:]) muscles_states = biorbd.VecBiorbdMuscleStateDynamics(nlp["nbMuscle"]) muscles_activations = controls for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(muscles_activations[k]) muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx() qddot = biorbd.Model.ForwardDynamicsConstraintsDirect( nlp["model"], q, qdot, muscles_tau).to_mx() qdot_reduced = nlp["q_mapping"].reduce.map(qdot) qddot_reduced = nlp["q_dot_mapping"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced)
def fcn_dyn_nocontact(x, u): activations = u[: params.nbMus] F = u[params.nbMus:] Q = x[:params.nbQ()] dQ = x[params.nbQ(): 2 * params.nbQ()] states = biorbd.VecBiorbdMuscleStateDynamics(params.nbMus) n_muscle = 0 for state in states: state.setActivation(activations[n_muscle]) # Set muscles activations n_muscle += 1 joint_torque = params.model_swing.muscularJointTorque(activations, Q, dQ) joint_torque[0] = F[0] joint_torque[1] = F[1] joint_torque[2] = F[2] ddQ = params.model_swing.ForwardDynamics(Q, dQ, joint_torque) return np.hstack(dQ, ddQ)
def forces_from_forward_dynamics_torque_muscle_driven_with_contact( states, controls, nlp): q, qdot, residual_tau = Dynamics.__dispatch_data(states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleStateDynamics(nlp["nbMuscle"]) muscles_activations = controls[nlp["nbTau"]:] for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(muscles_activations[k]) muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx() tau = muscles_tau + residual_tau cs = nlp["model"].getConstraints() biorbd.Model.ForwardDynamicsConstraintsDirect(nlp["model"], q, qdot, tau, cs) return cs.getForce().to_mx()
def plot_callback(callback, params, ubx, lbx, u0, x0, U_real, GRF_real, M_real): # INITIALISATION GRAPH WITH INITIAL GUESS upperbound_u = ubx[:params.nbU] lowerbound_u = lbx[:params.nbU] upperbound_x = ubx[params.nbU * params.nbNoeuds : params.nbU * params.nbNoeuds + params.nbX] lowerbound_x = lbx[params.nbU * params.nbNoeuds: params.nbU * params.nbNoeuds + params.nbX] # TIME t_stance = np.linspace(0, params.T_stance, params.nbNoeuds_stance) t_swing = t_stance[-1] + np.linspace(0, params.T_swing, params.nbNoeuds_swing) t = np.hstack([t_stance, t_swing]) def plot_control(ax, t, x): nbPoints = len(np.array(x)) for n in range(nbPoints - 1): ax.plot([t[n], t[n + 1], t[n + 1]], [x[n], x[n], x[n + 1]], 'b') # CONTROL fig1, axes1 = plt.subplots(5, 4, sharex=True, figsize=(10, 10)) # Create figure Labels = ['GLUT_MAX1', 'GLUT_MAX2', 'GLUT_MAX3', 'GLUT_MED1', 'GLUT_MED2', 'GLUT_MED3', 'R_SEMIMEM', 'R_SEMITEN', 'R_BI_FEM_LH', 'R_RECTUS_FEM', 'R_VAS_MED', 'R_VAS_INT', 'R_VAS_LAT', 'R_GAS_MED', 'R_GAS_LAT', 'R_SOLEUS', 'R_TIB_ANT', 'Pelvis Tx', 'Pelvis Ty', 'Pelvis Rz'] # Control labels axes1 = axes1.flatten() # Get axes figure (vector) u_emg = 9 # init variable for muscle with emg for i in range(params.nbU): ax = axes1[i] # get the correct subplot ax.set_title(Labels[i]) # put control label ax.plot([0, params.T], [lowerbound_u[i], lowerbound_u[i]], 'k--') # lower bound ax.plot([0, params.T], [upperbound_u[i], upperbound_u[i]], 'k--') # upper bound ax.plot([params.T_stance, params.T_stance], [lowerbound_u[i], upperbound_u[i]], 'k:') # end of the stance phase ax.grid(True) if (i != 1) and (i != 2) and (i != 3) and (i != 5) and (i != 6) and (i != 11) and (i != 12) and (i < (nbMus - 1)): ax.plot(t, U_real[u_emg, :], 'r') # plot emg if available u_emg -= 1 if (i > params.nbU - 5): ax.set_xlabel('time (s)') if (i < (params.nbMus - 1)): ax.yaxis.set_ticks(np.arange(0, 1, 0.5)) plot_control(ax, t, u0[i, :]) # plot initial guess # STATES ts = np.hstack([t_stance, t_swing, t_swing[-1] + (t_swing[-1] - t_swing[-2])]) # Adjusted time (T + dt) fig2, axes2 = plt.subplots(2, 6, sharex=True, figsize=(20, 10)) # Create figure axes2 = axes2.flatten() # Get axes figure (vector) Labels_X = ['Pelvis_TX', 'Pelvis_TY', 'Pelvis_RZ', 'Hip', 'Knee', 'Ankle'] # Control labels for q in range(params.nbQ): ax1 = axes2[q] ax1.set_title('Q ' + Labels_X[q]) if q != 0 and q != 1: ax1.plot([0, params.T], [lowerbound_x[q] * (180 / np.pi), lowerbound_x[q] * (180 / np.pi)], 'k--') # lower bound ax1.plot([0, params.T], [upperbound_x[q] * (180 / np.pi), upperbound_x[q] * (180 / np.pi)], 'k--') # upper bound ax1.plot([params.T_stance, params.T_stance], [lowerbound_x[q] * (180 / np.pi), upperbound_x[q] * (180 / np.pi)], 'k:') # end of the stance phase ax1.plot(ts, x0[q, :] * (180 / np.pi), 'b') # plot initial guess q (degre) else: ax1.plot([0, params.T], [lowerbound_x[q], lowerbound_x[q]], 'k--') ax1.plot([0, params.T], [upperbound_x[q], upperbound_x[q]], 'k--') ax1.plot([params.T_stance, params.T_stance], [lowerbound_x[q], upperbound_x[q]], 'k:') # end of the stance phase ax1.plot(ts, x0[q, :], 'b') # plot initial guess q (trans) ax1.grid(True) ax2 = axes2[q + params.nbQ] ax2.set_title('dQ ' + Labels_X[q]) ax2.plot([0, params.T], [lowerbound_x[q], lowerbound_x[q]], 'k--') # lower bound ax2.plot([0, params.T], [upperbound_x[q], upperbound_x[q]], 'k--') # upper bound ax2.plot([params.T_stance, params.T_stance], [lowerbound_x[q], upperbound_x[q]], 'k:') # end of the stance phase ax2.plot(ts, x0[(q + params.nbQ), :], 'b') # plot initial guess dq ax2.set_xlabel('time (s)') ax2.grid(True) # GROUND REACTION FORCES GRF = np.zeros((3, params.nbNoeuds)) m = biorbd.Model('/home/leasanchez/programmation/Simu_Marche_Casadi/ModelesS2M/ANsWER_Rleg_6dof_17muscle_1contact.bioMod') for k in range(params.nbNoeuds_stance): activations = u0[: params.nbMus] F = u0[params.nbMus:] Q = x0[:params.nbQ()] dQ = x0[params.nbQ(): 2 * params.nbQ()] states = biorbd.VecBiorbdMuscleStateDynamics(params.nbMus) n_muscle = 0 for state in states: state.setActivation(activations[n_muscle]) # Set muscles activations n_muscle += 1 joint_torque = m.muscularJointTorque(activations, Q, dQ) joint_torque[0] = F[0] joint_torque[1] = F[1] joint_torque[2] = F[2] C = m.getConstraints() m.ForwardDynamicsConstraintsDirect(Q, dQ, joint_torque, C) GRF[:, k] = C.getForce().to_array() # GROUND REACTION FORCES fig3, axes3 = plt.subplots(2, 1, sharex=True) axes3.flatten() ax_ap = axes3[0] ax_ap.set_title('GRF A/P during the gait') ax_ap.plot(t, GRF_real[1, :], 'r') ax_ap.plot([params.T_stance, params.T_stance], [min(GRF_real[1, :]), max(GRF_real[1, :])], 'k:') # end of the stance phase ax_ap.plot(t, GRF[0, :], 'b') ax_ap.grid(True) ax_v = axes3[1] ax_v.set_title('GRF vertical') ax_v.plot(t, GRF_real[2, :], 'r') ax_v.plot([params.T_stance, params.T_stance], [min(GRF_real[2, :]), max(GRF_real[2, :])], 'k:') ax_v.plot(t, GRF[2, :], 'b') ax_v.set_xlabel('time (s)') ax_v.grid(True) fig3.tight_layout() plt.show(block=False) def fcn_dyn_contact(x, u): activations = u[: params.nbMus] F = u[params.nbMus:] Q = x[:params.nbQ()] dQ = x[params.nbQ(): 2 * params.nbQ()] states = biorbd.VecBiorbdMuscleStateDynamics(params.nbMus) n_muscle = 0 for state in states: state.setActivation(activations[n_muscle]) # Set muscles activations n_muscle += 1 joint_torque = params.model_stance.muscularJointTorque(activations, Q, dQ) joint_torque[0] = F[0] joint_torque[1] = F[1] joint_torque[2] = F[2] ddQ = params.model_stance.ForwardDynamicsConstraintsDirect(Q, dQ, joint_torque) return np.hstack(dQ, ddQ) def fcn_dyn_nocontact(x, u): activations = u[: params.nbMus] F = u[params.nbMus:] Q = x[:params.nbQ()] dQ = x[params.nbQ(): 2 * params.nbQ()] states = biorbd.VecBiorbdMuscleStateDynamics(params.nbMus) n_muscle = 0 for state in states: state.setActivation(activations[n_muscle]) # Set muscles activations n_muscle += 1 joint_torque = params.model_swing.muscularJointTorque(activations, Q, dQ) joint_torque[0] = F[0] joint_torque[1] = F[1] joint_torque[2] = F[2] ddQ = params.model_swing.ForwardDynamics(Q, dQ, joint_torque) return np.hstack(dQ, ddQ) # Integration Runge Kutta 4 def int_RK4(fcn, T, nbNoeuds, x, u): dn = T / nbNoeuds # Time step for shooting point dt = dn / 4 # Time step for iteration xj = x for i in range(4): k1 = fcn(xj, u) x2 = xj + (dt / 2) * k1 k2 = fcn(x2, u) x3 = xj + (dt / 2) * k2 k3 = fcn(x3, u) x4 = xj + dt * k3 k4 = fcn(x4, u) xj += dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) return xj while plt.get_fignums(): # figures opened if callback.update_sol: # optimized values modified (new iteration) # STRUCTURE DATA sol_U = callback.sol_data[:params.nbU * params.nbNoeuds] sol_X = callback.sol_data[params.nbU * params.nbNoeuds: -params.nP] sol_q = np.hstack([sol_X[0::params.nbX], sol_X[1::params.nbX], sol_X[2::params.nbX], sol_X[3::params.nbX], sol_X[4::params.nbX], sol_X[5::params.nbX]]) sol_dq = np.hstack([sol_X[6::params.nbX], sol_X[7::params.nbX], sol_X[8::params.nbX], sol_X[9::params.nbX], sol_X[10::params.nbX], sol_X[11::params.nbX]]) sol_a = np.hstack([sol_U[0::params.nbU], sol_U[1::params.nbU], sol_U[2::params.nbU], sol_U[3::params.nbU], sol_U[4::params.nbU], sol_U[5::params.nbU], sol_U[6::params.nbU], sol_U[7::params.nbU], sol_U[8::params.nbU], sol_U[9::params.nbU], sol_U[10::params.nbU], sol_U[11::params.nbU], sol_U[12::params.nbU], sol_U[13::params.nbU], sol_U[14::params.nbU], sol_U[15::params.nbU], sol_U[16::params.nbU]]) sol_F = np.hstack([sol_U[17::params.nbU], sol_U[18::params.nbU], sol_U[19::params.nbU]]) # CONVERGENCE JR = Je = Jm = Ja = constraint = 0 GRF = np.zeros((3, params.nbNoeuds)) for k in range(params.nbNoeuds_stance): uk = np.array(sol_U[params.nbU * k : params.nbU * (k + 1)]) xk = np.array(sol_X[params.nbX * k : params.nbX * (k + 1)]) xk1 = np.array(sol_X[params.nbX * (k + 1) : params.nbX * (k + 2)]) # GROUND REACTION FORCES activations = uk[: params.nbMus] F = uk[params.nbMus:] Q = xk[:params.nbQ()] dQ = xk[params.nbQ(): 2 * params.nbQ()] states = biorbd.VecBiorbdMuscleStateDynamics(params.nbMus) n_muscle = 0 for state in states: state.setActivation(activations[n_muscle]) # Set muscles activations n_muscle += 1 joint_torque = params.model_stance.muscularJointTorque(activations, Q, dQ) joint_torque[0] = F[0] joint_torque[1] = F[1] joint_torque[2] = F[2] C = m.getConstraints() m.ForwardDynamicsConstraintsDirect(Q, dQ, joint_torque, C) GRF[:, k] = C.getForce().to_array() # OBJECTIF JR += params.wR * ((GRF[0, k] - GRF_real[1, k]) * (GRF[0, k] - GRF_real[1, k])) JR += params.wR * ((GRF[2, k] - GRF_real[2, k]) * (GRF[2, k] - GRF_real[2, k])) Ja += fcn_objective_activation(params.wL, uk) Je += fcn_objective_emg(params.wU, uk) markers = params.model_stance.markers(xk[:params.nbQ]) for nMark in range(params.nbMarkers): marker = markers[nMark].to_array() if params.model_stance.marker(nMark).isAnatomical(): Jm += params.wMa * ((marker[0] - M_real[0, nMark]) * (marker[0] - M_real[0, nMark])) # x Jm += params.wMa * ((marker[2] - M_real[2, nMark]) * (marker[2] - M_real[2, nMark])) # z else: Jm += params.wMt * ((marker[0] - M_real[0, nMark]) * (marker[0] - M_real[0, nMark])) Jm += params.wMt * ((marker[2] - M_real[2, nMark]) * (marker[2] - M_real[2, nMark])) # CONTRAINTES constraint += xk1 - int_RK4(fcn_dyn_contact, params.T_stance, params.nbNoeuds_stance, xk, uk) for k in range(params.nbNoeuds_swing): uk = np.array(sol_U[params.nbU * k : params.nbU * (k + 1)]) xk = np.array(sol_X[params.nbX * k : params.nbX * (k + 1)]) xk1 = np.array(sol_X[params.nbX * (k + 1) : params.nbX * (k + 2)]) # CONSTRAINT constraint += xk1 - int_RK4(fcn_dyn_nocontact, params.T_swing, params.nbNoeuds_swing, xk, uk) # OBJECTIF Ja += fcn_objective_activation(params.wL, uk) Je += fcn_objective_emg(params.wU, uk) markers = params.model_swing.markers(xk[:params.nbQ]) for nMark in range(params.nbMarkers): marker = markers[nMark].to_array() if params.model_swing.marker(nMark).isAnatomical(): Jm += params.wMa * ((marker[0] - M_real[0, nMark]) * (marker[0] - M_real[0, nMark])) # x Jm += params.wMa * ((marker[2] - M_real[2, nMark]) * (marker[2] - M_real[2, nMark])) # z else: Jm += params.wMt * ((marker[0] - M_real[0, nMark]) * (marker[0] - M_real[0, nMark])) Jm += params.wMt * ((marker[2] - M_real[2, nMark]) * (marker[2] - M_real[2, nMark])) J = Ja + Je + Jm + JR # PRINT VALUES print('\n \nGlobal : ' + str(J)) print('activation : ' + str(Ja)) print('emg : ' + str(Je)) print('marker : ' + str(Jm)) print('ground reaction forces : ' + str(JR)) print('constraints : ' + str(sum(constraint)) + '\n') def plot_control_update(ax, t, x): nbPoints = len(np.array(x)) for n in range(nbPoints - 1): lines = ax.get_lines() if size(lines) > 52: lines[4 + n].set_xdata([t[n], t[n + 1], t[n + 1]]) lines[4 + n].set_ydata([x[n], x[n], x[n + 1]]) else: lines[3 + n].set_xdata([t[n], t[n + 1], t[n + 1]]) lines[3 + n].set_ydata([x[n], x[n], x[n + 1]]) # CONTROL axes1 = plt.figure(1).axes for i in range(params.nbMus): ax = axes1[i] plot_control_update(ax, t, sol_a[:, i]) for j in range(3): ax = axes1[i + 1 + j] plot_control_update(ax, t, sol_F[:, j]) # STATE axes2 = plt.figure(2).axes for q in range(params.nbQ): ax1 = axes2[q] lines = ax1.get_lines() if q != 0 and q != 1: lines[3].set_ydata(sol_q[:, q] * (180 / np.pi)) else: lines[3].set_ydata(sol_q[:, q]) for dq in range(params.nbQ): ax1 = axes2[q + 1 + dq] lines = ax1.get_lines() lines[3].set_ydata(sol_dq[:, dq]) # GRF axes3 = plt.figure(3).axes ax_ap = axes3[0] lines = ax_ap.get_lines() lines[2].set_ydata(GRF[0, :]) ax_v = axes3[1] lines = ax_v.get_lines() lines[2].set_ydata(GRF[2, :]) callback.update_sol = False # can get new iteration plt.draw() # update plots plt.pause(.001)
X0 = vertcat(*X0.T) GRF = [] # ------------ PHASE 1 : Stance phase for k in range(nbNoeuds_stance): Uk = u0[nbU*k : nbU*(k + 1)] Xk = X0[nbX*k: nbX*(k + 1)] Q = np.array(Xk[:nbQ]).squeeze() # states dQ = np.array(Xk[nbQ:2 * nbQ]).squeeze() activations = Uk[: nbMus] # controls F = np.array(Uk[nbMus :]) # DYNAMIQUE states = biorbd.VecBiorbdMuscleStateDynamics(nbMus) n_muscle = 0 for state in states: state.setActivation(double(activations[n_muscle])) # Set muscles activations n_muscle += 1 torque = model_stance.muscularJointTorque(states, Q, dQ).to_array() torque[0] = F[0] torque[1] = F[1] torque[2] = F[2] C = model_stance.getConstraints() ddQ = model_stance.ForwardDynamicsConstraintsDirect(Q, dQ, torque, C) GRFk = C.getForce().to_array() GRF.append(GRFk)