def forces_from_forward_dynamics_muscle_excitations_and_torque_driven_with_contact( states, controls, parameters, nlp): """ Returns contact forces computed from forward dynamics with contact force (forward_dynamics_muscle_excitations_and_torque_driven_with_contact) :param states: States. (MX.sym from CasADi) :param controls: Controls. (MX.sym from CasADi) :param nlp: An OptimalControlProgram class. :return: Contact forces. (MX.sym from CasADi) """ DynamicsFunctions.apply_parameters(parameters, nlp) q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data( states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_excitation = controls[nlp.shape["tau"]:] muscles_activations = states[nlp.shape["q"] + nlp.shape["q_dot"]:] for k in range(nlp.shape["muscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) 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 forward_dynamics_muscle_excitations_driven(states, controls, parameters, nlp): """ Forward dynamics (q, qdot, qddot -> tau) without external forces driven by muscle excitation (controls). :param states: States. (MX.sym from CasADi) :param controls: Controls. (MX.sym from CasADi) :param nlp: An OptimalControlProgram class. :return: Vertcat of derived states. (MX.sym from CasADi) """ DynamicsFunctions.apply_parameters(parameters, nlp) nq = nlp.mapping["q"].reduce.len q = nlp.mapping["q"].expand.map(states[:nq]) qdot = nlp.mapping["q_dot"].expand.map(states[nq:]) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_excitation = controls muscles_activations = states[nlp.shape["q"] + nlp.shape["q_dot"]:] for k in range(nlp.shape["muscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) muscles_states[k].setActivation(muscles_activations[k]) muscles_activations_dot = nlp.model.activationDot( muscles_states).to_mx() muscles_tau = nlp.model.muscularJointTorque(muscles_states, q, qdot).to_mx() qddot = biorbd.Model.ForwardDynamicsConstraintsDirect( nlp.model, q, qdot, muscles_tau).to_mx() q_dot = nlp.model.computeQdot(q, qdot).to_mx() qdot_reduced = nlp.mapping["q"].reduce.map(q_dot) qddot_reduced = nlp.mapping["q_dot"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forward_dynamics_muscle_excitations_and_torque_driven_with_contact( states, controls, parameters, nlp): """ Forward dynamics (q, qdot, qddot -> tau) with contact force driven by muscle excitation and joint torques (controls). :param states: States. (MX.sym from CasADi) :param controls: Controls. (MX.sym from CasADi) :param nlp: An OptimalControlProgram class. :return: Vertcat of derived states. (MX.sym from CasADi) """ DynamicsFunctions.apply_parameters(parameters, nlp) q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data( states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_excitation = controls[nlp.shape["tau"]:] muscles_activations = states[nlp.shape["q"] + nlp.shape["q_dot"]:] for k in range(nlp.shape["muscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) muscles_states[k].setActivation(muscles_activations[k]) muscles_activations_dot = nlp.model.activationDot( muscles_states).to_mx() 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() q_dot = nlp.model.computeQdot(q, qdot).to_mx() qdot_reduced = nlp.mapping["q"].reduce.map(q_dot) qddot_reduced = nlp.mapping["q_dot"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def muscle_forces(q: MX, qdot: MX, a: MX, controls: MX, model: biorbd.Model, use_activation=True): """ Compute muscle force Parameters ---------- q: MX Symbolic value of joint angle qdot: MX Symbolic value of joint velocity a: MX Symbolic activation controls: int Symbolic value of activations model: biorbd.Model biorbd model build with the bioMod use_activation: bool True if activation drive False if excitation driven Returns ------- List of muscle forces """ muscle_states = biorbd.VecBiorbdMuscleState(model.nbMuscles()) for k in range(model.nbMuscles()): if use_activation: muscle_states[k].setActivation(controls[k]) else: muscle_states[k].setActivation(a[k]) muscle_states[k].setExcitation(controls[k]) return model.muscleForces(muscle_states, q, qdot).to_mx()
def forward_dynamics_torque_muscle_driven(states, controls, parameters, nlp): """ Forward dynamics (q, qdot, qddot -> tau) without external forces driven by joint torques and muscles (controls). :param states: States. (MX.sym from CasADi) :param controls: Controls. (MX.sym from CasADi) :param nlp: An OptimalControlProgram class. :return: Vertcat of derived states. (MX.sym from CasADi) """ DynamicsFunctions.apply_parameters(parameters, nlp) q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data( states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleState(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.ForwardDynamics(nlp["model"], q, qdot, tau).to_mx() q_dot = nlp["model"].computeQdot(q, qdot).to_mx() qdot_reduced = nlp["q_mapping"].reduce.map(q_dot) qddot_reduced = nlp["q_dot_mapping"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced)
def muscles_forces(states, controls, nlp, force=None, len=None, tsl=None, pa=None, insx=None, insy=None, insz=None): nq = nlp["q_mapping"].reduce.len q = nlp["q_mapping"].expand.map(states[:nq]) qdot = nlp["q_dot_mapping"].expand.map(states[nq:]) biorbd_model = biorbd.Model("arm_Belaise.bioMod") activations = states[nlp["nbQ"] + nlp["nbQdot"]:] muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"]) for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(activations[k]) muscles_states[k].setExcitation(controls[k]) if force is not None: biorbd_model.muscle(k).characteristics().setForceIsoMax(force[k]) if len is not None: biorbd_model.muscle(k).characteristics().setOptimalLength(len[k]) if tsl is not None: biorbd_model.muscle(k).characteristics().setTendonSlackLength(tsl[k]) if pa is not None: biorbd_model.muscle(k).characteristics().setPennationAngle(pa[k]) if insx is not None: biorbd_model.muscle(k).position().setInsertionInLocal(biorbd.Vector3d(insx[k], insy[k], insz[k])) return biorbd_model.muscleForces(muscles_states, q, qdot).to_mx()
def muscles_forces(q, qdot, act, controls, model): muscles_states = biorbd.VecBiorbdMuscleState(model.nbMuscles()) for k in range(model.nbMuscles()): muscles_states[k].setExcitation(controls[k]) muscles_states[k].setActivation(act[k]) # muscles_tau = model.muscularJointTorque(muscles_states, True, q, qdot).to_mx() muscles_force = model.muscleForces(muscles_states, q, qdot).to_mx() return muscles_force
def muscles_force(states, controls, nlp): nq = nlp.mapping['q'].reduce.len q = nlp.mapping['q'].expand.map(states[:nq]) qdot = nlp.mapping['q_dot'].expand.map(states[nq:]) muscles_states = biorbd.VecBiorbdMuscleState(nlp.model.nbMuscles()) for k in range(nlp.model.nbMuscles()): muscles_states[k].setActivation(controls[k]) return biorbd_model.muscleForces(muscles_states, q, qdot).to_mx()
def muscles_forces(q, qdot, act, controls, model, use_activation=False): muscles_states = biorbd.VecBiorbdMuscleState(model.nbMuscles()) for k in range(model.nbMuscles()): if use_activation: muscles_states[k].setActivation(controls[k]) else: muscles_states[k].setExcitation(controls[k]) muscles_states[k].setActivation(act[k]) muscles_force = model.muscleForces(muscles_states, q, qdot).to_mx() return muscles_force
def muscles_tau(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:]) activations = states[nlp["nbQ"] + nlp["nbQdot"]:] muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"]) for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(activations[k]) muscles_states[k].setExcitation(controls[k]) muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx() return muscles_tau
def forward_dynamics_muscle_excitations_driven(states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp) -> MX: """ Forward dynamics driven by muscle excitations. Parameters ---------- states: MX.sym The state of the system controls: MX.sym The controls of the system parameters: MX.sym The parameters of the system nlp: NonLinearProgram The definition of the system Returns ---------- MX.sym The derivative of the states """ DynamicsFunctions.apply_parameters(parameters, nlp) nq = nlp.mapping["q"].to_first.len q = nlp.mapping["q"].to_second.map(states[:nq]) qdot = nlp.mapping["qdot"].to_second.map(states[nq:]) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_excitation = controls muscles_activations = states[nlp.shape["q"] + nlp.shape["qdot"]:] for k in range(nlp.shape["muscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) muscles_states[k].setActivation(muscles_activations[k]) muscles_activations_dot = nlp.model.activationDot( muscles_states).to_mx() muscles_tau = nlp.model.muscularJointTorque(muscles_states, q, qdot).to_mx() qddot = biorbd.Model.ForwardDynamicsConstraintsDirect( nlp.model, q, qdot, muscles_tau).to_mx() qdot = nlp.model.computeQdot(q, qdot).to_mx() qdot_reduced = nlp.mapping["q"].to_first.map(qdot) qddot_reduced = nlp.mapping["qdot"].to_first.map(qddot) return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forward_dynamics_muscle_excitations_and_torque_driven_with_contact( states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp) -> MX: """ Forward dynamics driven by muscle excitations and joint torques with contact constraints.. Parameters ---------- states: MX.sym The state of the system controls: MX.sym The controls of the system parameters: MX.sym The parameters of the system nlp: NonLinearProgram The definition of the system Returns ---------- MX.sym The derivative of the states """ DynamicsFunctions.apply_parameters(parameters, nlp) q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data( states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_excitation = controls[nlp.shape["tau"]:] muscles_activations = states[nlp.shape["q"] + nlp.shape["qdot"]:] for k in range(nlp.shape["muscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) muscles_states[k].setActivation(muscles_activations[k]) muscles_activations_dot = nlp.model.activationDot( muscles_states).to_mx() 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 = nlp.model.computeQdot(q, qdot).to_mx() qdot_reduced = nlp.mapping["q"].to_first.map(qdot) qddot_reduced = nlp.mapping["qdot"].to_first.map(qddot) return vertcat(qdot_reduced, qddot_reduced, muscles_activations_dot)
def forces_from_forward_dynamics_muscle_excitations_and_torque_driven_with_contact( states: MX.sym, controls: MX.sym, parameters: MX.sym, nlp) -> MX: """ Contact forces of a forward dynamics driven by muscle excitations and joint torques with contact constraints. Parameters ---------- states: MX.sym The state of the system controls: MX.sym The controls of the system parameters: MX.sym The parameters of the system nlp: NonLinearProgram The definition of the system Returns ---------- MX.sym The contact forces that ensure no acceleration at these contact points """ DynamicsFunctions.apply_parameters(parameters, nlp) q, qdot, residual_tau = DynamicsFunctions.dispatch_q_qdot_tau_data( states, controls, nlp) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_excitation = controls[nlp.shape["tau"]:] muscles_activations = states[nlp.shape["q"] + nlp.shape["qdot"]:] for k in range(nlp.shape["muscle"]): muscles_states[k].setExcitation(muscles_excitation[k]) 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 forward_dynamics_muscle_activations_driven(states, controls, parameters, nlp): DynamicsFunctions.apply_parameters(parameters, nlp) nq = nlp.mapping["q"].reduce.len q = nlp.mapping["q"].expand.map(states[:nq]) qdot = nlp.mapping["q_dot"].expand.map(states[nq:]) muscles_states = biorbd.VecBiorbdMuscleState(nlp.shape["muscle"]) muscles_activations = controls for k in range(nlp.shape["muscle"]): 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() q_dot = nlp.model.computeQdot(q, qdot).to_mx() qdot_reduced = nlp.mapping["q"].reduce.map(q_dot) qddot_reduced = nlp.mapping["q_dot"].reduce.map(qddot) return vertcat(qdot_reduced, qddot_reduced)
# # Please note that this example will work only with the Eigen backend # # Load a predefined model model = biorbd.Model("../arm26.bioMod") nq = model.nbQ() nqdot = model.nbQdot() nmus = model.nbMuscles() # Choose a position/velocity to compute dynamics from Q = np.zeros((nq, )) Qdot = np.zeros((nqdot, )) # Set all muscles to half of their maximal activation emg = biorbd.VecBiorbdMuscleState() for i in range(nmus): emg.append(biorbd.State(0, 0.5)) # Proceed with the computation of joint torque from the muscles Tau = model.muscularJointTorque(emg, Q, Qdot) # Compute the generalized accelerations using the Tau from muscles. # Please note that in forward dynamics setting, it is usually advised to # additionnal residual torques. You would add them here to Tau. Qddot = model.ForwardDynamics(Q, Qdot, Tau) # Print them to the console print(Qddot.to_array()) # As an extra, let's print the individual muscle forces
def xia_model_dynamic(states, controls, parameters, nlp): nbq = nlp["model"].nbQ() nbqdot = nlp["model"].nbQdot() nb_q_qdot = nbq + nbqdot q = states[:nbq] qdot = states[nbq:nb_q_qdot] active_fibers = states[nb_q_qdot:nb_q_qdot + nlp["nbMuscle"]] fatigued_fibers = states[nb_q_qdot + nlp["nbMuscle"]:nb_q_qdot + 2 * nlp["nbMuscle"]] resting_fibers = states[nb_q_qdot + 2 * nlp["nbMuscle"]:] residual_tau = controls[:nlp["nbTau"]] activation = controls[nlp["nbTau"]:] command = MX() comp = 0 for i in range(nlp["model"].nbMuscleGroups()): for k in range(nlp["model"].muscleGroup(i).nbMuscles()): develop_factor = ( nlp["model"].muscleGroup(i).muscle(k).characteristics( ).fatigueParameters().developFactor().to_mx()) recovery_factor = ( nlp["model"].muscleGroup(i).muscle(k).characteristics( ).fatigueParameters().recoveryFactor().to_mx()) command = vertcat( command, if_else( lt(active_fibers[comp], activation[comp]), (if_else( gt(resting_fibers[comp], activation[comp] - active_fibers[comp]), develop_factor * (activation[comp] - active_fibers[comp]), develop_factor * resting_fibers[comp], )), recovery_factor * (activation[comp] - active_fibers[comp]), ), ) comp += 1 restingdot = -command + Muscles.r * Muscles.R * fatigued_fibers # todo r=r when activation=0 activatedot = command - Muscles.F * active_fibers fatiguedot = Muscles.F * active_fibers - Muscles.R * fatigued_fibers muscles_states = biorbd.VecBiorbdMuscleState(nlp["nbMuscle"]) for k in range(nlp["nbMuscle"]): muscles_states[k].setActivation(active_fibers[k]) # todo fix force max muscles_tau = nlp["model"].muscularJointTorque(muscles_states, q, qdot).to_mx() # todo get muscle forces and multiply them by activate [k] and same as muscularJointTorque tau = muscles_tau + residual_tau dxdt = MX(nlp["nx"], nlp["ns"]) if "external_forces" in nlp: for i, f_ext in enumerate(nlp["external_forces"]): qddot = biorbd.Model.ForwardDynamics(nlp["model"], q, qdot, tau, f_ext).to_mx() dxdt[:, i] = vertcat(qdot, qddot, activatedot, fatiguedot, restingdot) else: qddot = biorbd.Model.ForwardDynamics(nlp["model"], q, qdot, tau).to_mx() dxdt = vertcat(qdot, qddot, activatedot, fatiguedot, restingdot) return dxdt