def maximal_tau(nodes: PenaltyNodes, minimal_tau): nlp = nodes.nlp nq = nlp.mapping["q"].to_first.len q = [nlp.mapping["q"].to_second.map(mx[:nq]) for mx in nodes.x] qdot = [nlp.mapping["qdot"].to_second.map(mx[nq:]) for mx in nodes.x] min_bound = [] max_bound = [] func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q, nlp.qdot) for n in range(len(nodes.u)): bound = func(q[n], qdot[n]) min_bound.append(nlp.mapping["tau"].to_first.map( if_else(lt(bound[:, 1], minimal_tau), minimal_tau, bound[:, 1]))) max_bound.append(nlp.mapping["tau"].to_first.map( if_else(lt(bound[:, 0], minimal_tau), minimal_tau, bound[:, 0]))) obj = vertcat(*nodes.u) min_bound = vertcat(*min_bound) max_bound = vertcat(*max_bound) return ( vertcat(np.zeros(min_bound.shape), np.ones(max_bound.shape) * -np.inf), vertcat(obj + min_bound, obj - max_bound), vertcat(np.ones(min_bound.shape) * np.inf, np.zeros(max_bound.shape)), )
def tau_actuator_constraints(ocp, nlp, t, x, u, p, minimal_tau=None): nq = nlp.mapping["q"].reduce.len q = [nlp.mapping["q"].expand.map(mx[:nq]) for mx in x] q_dot = [nlp.mapping["q_dot"].expand.map(mx[nq:]) for mx in x] min_bound = [] max_bound = [] func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q, nlp.q_dot) for i in range(len(u)): bound = func(q[i], q_dot[i]) if minimal_tau: min_bound.append(nlp.mapping["tau"].reduce.map( if_else(lt(bound[:, 1], minimal_tau), minimal_tau, bound[:, 1]))) max_bound.append(nlp.mapping["tau"].reduce.map( if_else(lt(bound[:, 0], minimal_tau), minimal_tau, bound[:, 0]))) else: min_bound.append(nlp.mapping["tau"].reduce.map(bound[:, 1])) max_bound.append(nlp.mapping["tau"].reduce.map(bound[:, 0])) obj = vertcat(*u) min_bound = vertcat(*min_bound) max_bound = vertcat(*max_bound) return ( vertcat(np.zeros(min_bound.shape), np.ones(max_bound.shape) * -np.inf), vertcat(obj + min_bound, obj - max_bound), vertcat(np.ones(min_bound.shape) * np.inf, np.zeros(max_bound.shape)), )
def custom_func_track_markers(pn: PenaltyNodes, first_marker_idx: int, second_marker_idx: int) -> MX: """ The used-defined constraint function (This particular one mimics the ConstraintFcn.SUPERIMPOSE_MARKERS) Except for the last two Parameters ---------- pn: PenaltyNodes The penalty node elements first_marker_idx: int The index of the first marker in the bioMod second_marker_idx: int The index of the second marker in the bioMod Returns ------- The cost that should be minimize in the MX format. If the cost is quadratic, do not put the square here, but use the quadratic=True parameter instead """ nq = pn.nlp.shape["q"] val = [] markers_func = biorbd.to_casadi_func("markers", pn.nlp.model.markers, pn.nlp.q) for v in pn.x: q = v[:nq] markers = markers_func(q) first_marker = markers[:, first_marker_idx] second_marker = markers[:, second_marker_idx] val = vertcat(val, first_marker - second_marker) return val
def impact(ocp, transition): """ TODO """ if ocp.nlp[transition.phase_pre_idx]["nx"] != ocp.nlp[ (transition.phase_pre_idx + 1) % ocp.nb_phases]["nx"]: raise RuntimeError( "Impact transition without same nx is not possible, please provide a custom state transition" ) # Aliases nlp_pre, nlp_post = StateTransitionFunctions.Functions.__get_nlp_pre_and_post( ocp, transition.phase_pre_idx) nbQ = nlp_pre["nbQ"] nbQdot = nlp_pre["nbQdot"] q = nlp_pre["q_mapping"].expand.map(nlp_pre["X"][-1][:nbQ]) qdot_pre = nlp_pre["q_dot_mapping"].expand.map( nlp_pre["X"][-1][nbQ:nbQ + nbQdot]) if nlp_post["model"].nbContacts() == 0: warn("The chosen model does not have any contact") # A new model is loaded here so we can use pre Qdot with post model, this is a hack and should be dealt # a better way (e.g. create a supplementary variable in V that link the pre and post phase with a # constraint. The transition would therefore apply to node_0 and node_1 (with an augmented ns) model = biorbd.Model( nlp_post["model"].path().absolutePath().to_string()) func = biorbd.to_casadi_func("impulse_direct", model.ComputeConstraintImpulsesDirect, nlp_pre["q"], nlp_pre["qdot"]) qdot_post = func(q, qdot_pre) qdot_post = nlp_post["q_dot_mapping"].reduce.map(qdot_post) val = nlp_pre["X"][-1][:nbQ] - nlp_post["X"][0][:nbQ] val = vertcat(val, qdot_post - nlp_post["X"][0][nbQ:nbQ + nbQdot]) return val
def com_dot_z(ocp, nlp, t, x, u, p): q = nlp.mapping["q"].expand.map(x[0][:nlp.shape["q"]]) q_dot = nlp.mapping["q"].expand.map(x[0][nlp.shape["q"]:]) com_dot_func = biorbd.to_casadi_func("Compute_CoM_dot", nlp.model.CoMdot, nlp.q, nlp.q_dot) com_dot = com_dot_func(q, q_dot) return com_dot[2]
def heel_on_floor(nodes: PenaltyNodes): nlp = nodes.nlp nb_q = nlp.shape["q"] q_reduced = nodes.x[0][:nb_q] q = nlp.mapping["q"].to_second.map(q_reduced) marker_func = biorbd.to_casadi_func("heel_on_floor", nlp.model.marker, nlp.q, 3) tal_marker_z = marker_func(q)[2] return tal_marker_z + 0.779 # floor = -0.77865829
def com_dot_z(nodes: PenaltyNodes): nlp = nodes.nlp x = nodes.x q = nlp.mapping["q"].to_second.map(x[0][:nlp.shape["q"]]) qdot = nlp.mapping["q"].to_second.map(x[0][nlp.shape["q"]:]) com_dot_func = biorbd.to_casadi_func("Compute_CoM_dot", nlp.model.CoMdot, nlp.q, nlp.qdot) com_dot = com_dot_func(q, qdot) return com_dot[2]
def impact(ocp, transition: PhaseTransition) -> MX: """ A discontinuous function that simulates an inelastic impact of a new contact point Parameters ---------- ocp: OptimalControlProgram A reference to the ocp transition: PhaseTransition A reference to the phase transition Returns ------- The difference between the last and first node after applying the impulse equations """ if ( ocp.nlp[transition.phase_pre_idx].states.shape != ocp.nlp[(transition.phase_pre_idx + 1) % ocp.n_phases].states.shape ): raise RuntimeError( "Impact transition without same nx is not possible, please provide a custom phase transition" ) # Aliases nlp_pre, nlp_post = PhaseTransitionFunctions.Functions.__get_nlp_pre_and_post(ocp, transition.phase_pre_idx) n_q = len(nlp_pre.states["q"]) n_qdot = len(nlp_pre.states["qdot"]) q = DynamicsFunctions.get(nlp_pre.states["q"], nlp_pre.X[-1]) qdot_pre = DynamicsFunctions.get(nlp_pre.states["qdot"], nlp_pre.X[-1]) if nlp_post.model.nbContacts() == 0: warn("The chosen model does not have any contact") # A new model is loaded here so we can use pre Qdot with post model, this is a hack and should be dealt # a better way (e.g. create a supplementary variable in v that link the pre and post phase with a # constraint. The transition would therefore apply to node_0 and node_1 (with an augmented ns) model = biorbd.Model(nlp_post.model.path().absolutePath().to_string()) func = biorbd.to_casadi_func( "impulse_direct", model.ComputeConstraintImpulsesDirect, nlp_pre.states["q"].mx, nlp_pre.states["qdot"].mx, ) qdot_post = func(q, qdot_pre) qdot_post = nlp_post.variable_mappings["qdot"].to_first.map(qdot_post) val = [] for key in nlp_pre.states: if key != "qdot": # Continuity constraint var_pre = DynamicsFunctions.get(nlp_pre.states[key], nlp_pre.X[-1]) var_post = DynamicsFunctions.get(nlp_post.states[key], nlp_post.X[0]) val = vertcat(val, var_pre - var_post) else: var_post = DynamicsFunctions.get(nlp_post.states[key], nlp_post.X[0]) val = vertcat(val, qdot_post - var_post) return val
def heel_on_floor(ocp, nlp, t, x, u, p): # floor = -0.77865829 nb_q = nlp.shape["q"] q_reduced = nlp.X[0][:nb_q] q = nlp.mapping["q"].expand.map(q_reduced) marker_func = biorbd.to_casadi_func("heel_on_floor", nlp.model.marker, nlp.q, 3) tal_marker_z = marker_func(q)[2] return tal_marker_z + 0.779
def torque_max_from_actuators( constraint: Constraint, all_pn: PenaltyNodeList, min_torque=None, ): """ Non linear maximal values of joint torques computed from the torque-position-velocity relationship Parameters ---------- constraint: Constraint The actual constraint to declare all_pn: PenaltyNodeList The penalty node elements min_torque: float Minimum joint torques. This prevent from having too small torques, but introduces an if statement """ # TODO: Add index to select the u (control_idx) nlp = all_pn.nlp q = [ nlp.variable_mappings["q"].to_second.map( mx[nlp.states["q"].index, :]) for mx in all_pn.x ] qdot = [ nlp.variable_mappings["qdot"].to_second.map( mx[nlp.states["qdot"].index, :]) for mx in all_pn.x ] if min_torque and min_torque < 0: raise ValueError( "min_torque cannot be negative in tau_max_from_actuators") func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.states["q"].mx, nlp.states["qdot"].mx) constraint.min_bound = np.repeat([0, -np.inf], nlp.controls.shape) constraint.max_bound = np.repeat([np.inf, 0], nlp.controls.shape) for i in range(len(all_pn.u)): bound = func(q[i], qdot[i]) if min_torque: min_bound = nlp.variable_mappings["tau"].to_first.map( if_else(lt(bound[:, 1], min_torque), min_torque, bound[:, 1])) max_bound = nlp.variable_mappings["tau"].to_first.map( if_else(lt(bound[:, 0], min_torque), min_torque, bound[:, 0])) else: min_bound = nlp.variable_mappings["tau"].to_first.map( bound[:, 1]) max_bound = nlp.variable_mappings["tau"].to_first.map( bound[:, 0]) ConstraintFunction.add_to_penalty( all_pn.ocp, all_pn, vertcat( *[all_pn.u[i] + min_bound, all_pn.u[i] - max_bound]), constraint)
def test_CoM(): m = biorbd.Model("../../models/pyomecaman.bioMod") q = np.array( [0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) q_ddot = np.array([10, 10, 10, 30, 30, 30, 30, 30, 30, 30, 30, 30, 30]) expected_CoM = np.array( [-0.0034679564024098523, 0.15680579877453169, 0.07808112642459612]) expected_CoM_dot = np.array( [-0.05018973433722229, 1.4166208451420528, 1.4301750486035787]) expected_CoM_ddot = np.array( [-0.7606169667295027, 11.508107073695976, 16.58853835505851]) if biorbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1) q_ddot_sym = MX.sym("q_ddot", m.nbQddot(), 1) CoM_func = biorbd.to_casadi_func("Compute_CoM", m.CoM, q_sym) CoM_dot_func = biorbd.to_casadi_func("Compute_CoM_dot", m.CoMdot, q_sym, q_dot_sym) CoM_ddot_func = biorbd.to_casadi_func("Compute_CoM_ddot", m.CoMddot, q_sym, q_dot_sym, q_ddot_sym) CoM = np.array(CoM_func(q)) CoM_dot = np.array(CoM_dot_func(q, q_dot)) CoM_ddot = np.array(CoM_ddot_func(q, q_dot, q_ddot)) elif not biorbd.currentLinearAlgebraBackend(): # If Eigen backend is used CoM = m.CoM(q).to_array() CoM_dot = m.CoMdot(q, q_dot).to_array() CoM_ddot = m.CoMddot(q, q_dot, q_ddot).to_array() else: raise NotImplementedError("Backend not implemented in test") np.testing.assert_almost_equal(CoM.squeeze(), expected_CoM) np.testing.assert_almost_equal(CoM_dot.squeeze(), expected_CoM_dot) np.testing.assert_almost_equal(CoM_ddot.squeeze(), expected_CoM_ddot)
def test_forward_dynamics_with_external_forces(): m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod") q = np.array([i * 1.1 for i in range(m.nbQ())]) qdot = np.array([i * 1.1 for i in range(m.nbQ())]) tau = np.array([i * 1.1 for i in range(m.nbQ())]) # With external forces if biorbd.currentLinearAlgebraBackend() == 1: from casadi import MX sv1 = MX((11.1, 22.2, 33.3, 44.4, 55.5, 66.6)) sv2 = MX((11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2)) else: sv1 = np.array((11.1, 22.2, 33.3, 44.4, 55.5, 66.6)) sv2 = np.array( (11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2)) f_ext = biorbd.VecBiorbdSpatialVector([ biorbd.SpatialVector(sv1), biorbd.SpatialVector(sv2), ]) if biorbd.currentLinearAlgebraBackend() == 1: q_sym = MX.sym("q", m.nbQ(), 1) qdot_sym = MX.sym("qdot", m.nbQdot(), 1) tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1) ForwardDynamics = biorbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym, f_ext) qddot = ForwardDynamics(q, qdot, tau) qddot = np.array(qddot)[:, 0] elif biorbd.currentLinearAlgebraBackend() == 0: # if Eigen backend is used qddot = m.ForwardDynamics(q, qdot, tau, f_ext).to_array() else: raise NotImplementedError("Backend not implemented in test") qddot_expected = np.array([ 8.8871711208009998, -13.647827029817943, -33.606145294752132, 16.922669487341341, -21.882821189868423, 41.15364990805439, 68.892537246574463, -324.59756885799197, -447.99217990207387, 18884.241415786601, -331.24622725851572, 1364.7620674666462, 3948.4748602722384, ]) np.testing.assert_almost_equal(qddot, qddot_expected)
def custom_func_align_markers(ocp, nlp, t, x, u, p, first_marker_idx, second_marker_idx): nq = nlp.shape["q"] val = [] markers = biorbd.to_casadi_func("markers", nlp.model.markers, nlp.q) for v in x: q = v[:nq] first_marker = markers(q)[:, first_marker_idx] second_marker = markers(q)[:, second_marker_idx] val = vertcat(val, first_marker - second_marker) return val
def test_set_vector3d(): m = biorbd.Model("../../models/pyomecaman.bioMod") m.setGravity(np.array((0, 0, -2))) if biorbd.currentLinearAlgebraBackend() == 1: from casadi import MX get_gravity = biorbd.to_casadi_func("Compute_Markers", m.getGravity)()["o0"] else: get_gravity = m.getGravity().to_array() assert get_gravity[2] == -2
def test_markers(): m = biorbd.Model("../../models/pyomecaman.bioMod") q = np.array( [0.1, 0.1, 0.1, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3, 0.3]) q_dot = np.array([1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3]) expected_markers_last = np.array([-0.11369, 0.63240501, -0.56253268]) expected_markers_last_dot = np.array([0.0, 4.16996219, 3.99459262]) if biorbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) q_dot_sym = MX.sym("q_dot", m.nbQdot(), 1) markers_func = biorbd.to_casadi_func("Compute_Markers", m.markers, q_sym) markersVelocity_func = biorbd.to_casadi_func("Compute_MarkersVelocity", m.markersVelocity, q_sym, q_dot_sym) markers = np.array(markers_func(q)) markers_dot = np.array(markersVelocity_func(q, q_dot)) elif not biorbd.currentLinearAlgebraBackend(): # If Eigen backend is used markers = np.array([mark.to_array() for mark in m.markers(q)]).T markers_dot = np.array( [mark.to_array() for mark in m.markersVelocity(q, q_dot)]).T else: raise NotImplementedError("Backend not implemented in test") np.testing.assert_almost_equal(markers[:, -1], expected_markers_last) np.testing.assert_almost_equal(markers_dot[:, -1], expected_markers_last_dot)
def check_results(biorbd_model, N, Xs): # Casadi functions x = cas.MX.sym("x", biorbd_model.nbQ() + biorbd_model.nbQdot()) u = cas.MX.sym("u", 1) q = cas.MX.sym("q", biorbd_model.nbQ()) markers_kyn = biorbd.to_casadi_func("makers_kyn", biorbd_model.markers, q) Y_est = np.zeros( (3, biorbd_model.nbMarkers(), N)) # Measurements trajectory for n in range(N): Y_est[:, :, n] = markers_kyn(Xs[:biorbd_model.nbQ(), n]) return Y_est
def test_forward_dynamics(): m = biorbd.Model("../../models/pyomecaman_withActuators.bioMod") q = np.array([i * 1.1 for i in range(m.nbQ())]) qdot = np.array([i * 1.1 for i in range(m.nbQ())]) tau = np.array([i * 1.1 for i in range(m.nbQ())]) if biorbd.currentLinearAlgebraBackend() == 1: # If CasADi backend is used from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) qdot_sym = MX.sym("qdot", m.nbQdot(), 1) tau_sym = MX.sym("tau", m.nbGeneralizedTorque(), 1) ForwardDynamics = biorbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym) qddot = ForwardDynamics(q, qdot, tau) qddot = np.array(qddot)[:, 0] elif biorbd.currentLinearAlgebraBackend() == 0: # if Eigen backend is used qddot = m.ForwardDynamics(q, qdot, tau).to_array() else: raise NotImplementedError("Backend not implemented in test") qddot_expected = np.array([ 20.554883896960259, -22.317642013324736, -77.406439058256126, 17.382961188212313, -63.426361095191858, 93.816468824985876, 106.46105024484631, 95.116641811710167, -268.1961283528546, 2680.3632159799949, -183.4582596257801, 755.89411812405604, 163.60239754283589, ]) np.testing.assert_almost_equal(qddot, qddot_expected)
def torque_bounds(x, min_or_max, nlp, minimal_tau=None): q = nlp.mapping["q"].to_second.map(x[:7, :]) qdot = nlp.mapping["q"].to_second.map(x[7:, :]) func = biorbd.to_casadi_func("TorqueMax", nlp.model.torqueMax, nlp.q, nlp.qdot) res = [] for dof in [6, 7, 8, 9]: bound = [] for i in range(len(x[0])): tmp = func(q[:, i], qdot[:, i]) if minimal_tau and tmp[dof, min_or_max] < minimal_tau: bound.append(minimal_tau) else: bound.append(tmp[dof, min_or_max]) res.append(np.array(bound)) return np.array(res)
def imu_to_array(): m = biorbd.Model("../../models/IMUandCustomRT/pyomecaman_withIMUs.bioMod") q = np.zeros((m.nbQ(), )) if biorbd.currentLinearAlgebraBackend() == 1: from casadi import MX q_sym = MX.sym("q", m.nbQ(), 1) imu_func = biorbd.to_casadi_func("imu", m.IMU, q_sym) imu = imu_func(q)[:, :4] else: imu = m.IMU(q)[0].to_array() np.testing.assert_almost_equal( imu, np.array([[0.99003329, -0.09933467, 0.09983342, 0.26719], [0.10925158, 0.98903828, -0.09933467, 0.04783], [-0.08887169, 0.10925158, 0.99003329, -0.20946], [0., 0., 0., 1.]]))
def add_casadi_func(self, name: str, function: Callable, *all_param: Any) -> casadi.Function: """ Add to the pool of declared casadi function. If the function already exists, it is skipped Parameters ---------- name: str The unique name of the function to add to the casadi functions pool function: Callable The biorbd function to add all_param: dict Any parameters to pass to the biorbd function """ if name in self.casadi_func: return self.casadi_func[name] else: self.casadi_func[name] = biorbd.to_casadi_func( name, function, *all_param) return self.casadi_func[name]
def torque_max_from_actuators(constraint, ocp, nlp, t, x, u, p, min_torque=None): # TODO: Add index to select the u (control_idx) nq = nlp.mapping["q"].reduce.len q = [nlp.mapping["q"].expand.map(mx[:nq]) for mx in x] q_dot = [nlp.mapping["q_dot"].expand.map(mx[nq:]) for mx in x] if min_torque and min_torque < 0: raise ValueError( "min_torque cannot be negative in tau_max_from_actuators") func = biorbd.to_casadi_func("torqueMax", nlp.model.torqueMax, nlp.q, nlp.q_dot) constraint.min_bound = np.repeat([0, -np.inf], nlp.nu) constraint.max_bound = np.repeat([np.inf, 0], nlp.nu) for i in range(len(u)): bound = func(q[i], q_dot[i]) if min_torque: min_bound = nlp.mapping["tau"].reduce.map( if_else(lt(bound[:, 1], min_torque), min_torque, bound[:, 1])) max_bound = nlp.mapping["tau"].reduce.map( if_else(lt(bound[:, 0], min_torque), min_torque, bound[:, 0])) else: min_bound = nlp.mapping["tau"].reduce.map(bound[:, 1]) max_bound = nlp.mapping["tau"].reduce.map(bound[:, 0]) ConstraintFunction.add_to_penalty( ocp, nlp, vertcat(*[u[i] + min_bound, u[i] - max_bound]), constraint)
def generate_data(biorbd_model: biorbd.Model, final_time: float, n_shooting: int, use_residual_torque: bool = True) -> tuple: """ Generate random data. If np.random.seed is defined before, it will always return the same results Parameters ---------- biorbd_model: biorbd.Model The loaded biorbd model final_time: float The time at final node n_shooting: int The number of shooting points use_residual_torque: bool If residual torque are present or not in the dynamics Returns ------- The time, marker, states and controls of the program. The ocp will try to track these """ # Aliases n_q = biorbd_model.nbQ() n_qdot = biorbd_model.nbQdot() n_tau = biorbd_model.nbGeneralizedTorque() n_mus = biorbd_model.nbMuscleTotal() dt = final_time / n_shooting if use_residual_torque: nu = n_tau + n_mus else: nu = n_mus # Casadi related stuff symbolic_q = MX.sym("q", n_q, 1) symbolic_qdot = MX.sym("qdot", n_qdot, 1) symbolic_controls = MX.sym("u", nu, 1) symbolic_parameters = MX.sym("params", 0, 0) markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_q) nlp = NonLinearProgram() nlp.model = biorbd_model nlp.shape = {"muscle": n_mus} nlp.mapping = { "q": BiMapping(range(n_q), range(n_q)), "qdot": BiMapping(range(n_qdot), range(n_qdot)), } if use_residual_torque: nlp.shape["tau"] = n_tau nlp.mapping["tau"] = BiMapping(range(n_tau), range(n_tau)) dyn_func = DynamicsFunctions.forward_dynamics_muscle_activations_and_torque_driven else: dyn_func = DynamicsFunctions.forward_dynamics_muscle_activations_driven symbolic_states = vertcat(*(symbolic_q, symbolic_qdot)) dynamics_func = biorbd.to_casadi_func("ForwardDyn", dyn_func, symbolic_states, symbolic_controls, symbolic_parameters, nlp) def dyn_interface(t, x, u): if use_residual_torque: u = np.concatenate([np.zeros(n_tau), u]) return np.array(dynamics_func(x, u, np.empty((0, 0)))).squeeze() # Generate some muscle activation U = np.random.rand(n_shooting, n_mus).T # Integrate and collect the position of the markers accordingly X = np.ndarray((n_q + n_qdot, n_shooting + 1)) markers = np.ndarray((3, biorbd_model.nbMarkers(), n_shooting + 1)) def add_to_data(i, q): X[:, i] = q markers[:, :, i] = markers_func(q[0:n_q]) x_init = np.array([0] * n_q + [0] * n_qdot) add_to_data(0, x_init) for i, u in enumerate(U.T): sol = solve_ivp(dyn_interface, (0, dt), x_init, method="RK45", args=(u, )) x_init = sol["y"][:, -1] add_to_data(i + 1, x_init) time_interp = np.linspace(0, final_time, n_shooting + 1) return time_interp, markers, X, U
def _prepare_function_for_casadi(self): q_sym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.markers = biorbd.to_casadi_func("Markers", self.m.markers, q_sym)
def _prepare_function_for_casadi(self): Qsym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.CoMs = biorbd.to_casadi_func("CoMbySegment", self.m.CoMbySegmentInMatrix, Qsym)
def _prepare_function_for_casadi(self): Qsym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.CoM = biorbd.to_casadi_func("CoM", self.m.CoM, Qsym)
def _prepare_function_for_casadi(self): q_sym = casadi.MX.sym("Q", self.m.nbQ(), 1) self.contacts = biorbd.to_casadi_func("Contacts", self.m.constraintsInGlobal, q_sym, True)
def generate_data(biorbd_model, final_time, nb_shooting, use_residual_torque=True): # Aliases nb_q = biorbd_model.nbQ() nb_qdot = biorbd_model.nbQdot() nb_tau = biorbd_model.nbGeneralizedTorque() nb_mus = biorbd_model.nbMuscleTotal() nb_markers = biorbd_model.nbMarkers() dt = final_time / nb_shooting if use_residual_torque: nu = nb_tau + nb_mus else: nu = nb_mus # Casadi related stuff symbolic_q = MX.sym("q", nb_q, 1) symbolic_qdot = MX.sym("q_dot", nb_qdot, 1) symbolic_controls = MX.sym("u", nu, 1) symbolic_parameters = MX.sym("params", 0, 0) markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_q) nlp = NonLinearProgram( model=biorbd_model, shape={"muscle": nb_mus}, mapping={ "q": BidirectionalMapping(Mapping(range(nb_q)), Mapping(range(nb_q))), "q_dot": BidirectionalMapping(Mapping(range(nb_qdot)), Mapping(range(nb_qdot))), }, ) if use_residual_torque: nlp.shape["tau"] = nb_tau nlp.mapping["tau"] = BidirectionalMapping(Mapping(range(nb_tau)), Mapping(range(nb_tau))) dyn_func = DynamicsFunctions.forward_dynamics_torque_muscle_driven else: dyn_func = DynamicsFunctions.forward_dynamics_muscle_activations_driven symbolic_states = vertcat(*(symbolic_q, symbolic_qdot)) dynamics_func = biorbd.to_casadi_func("ForwardDyn", dyn_func, symbolic_states, symbolic_controls, symbolic_parameters, nlp) def dyn_interface(t, x, u): if use_residual_torque: u = np.concatenate([np.zeros(nb_tau), u]) return np.array(dynamics_func(x, u, np.empty((0, 0)))).squeeze() # Generate some muscle activation U = np.random.rand(nb_shooting, nb_mus).T # Integrate and collect the position of the markers accordingly X = np.ndarray((nb_q + nb_qdot, nb_shooting + 1)) markers = np.ndarray((3, biorbd_model.nbMarkers(), nb_shooting + 1)) def add_to_data(i, q): X[:, i] = q markers[:, :, i] = markers_func(q[0:nb_q]) x_init = np.array([0] * nb_q + [0] * nb_qdot) add_to_data(0, x_init) for i, u in enumerate(U.T): sol = solve_ivp(dyn_interface, (0, dt), x_init, method="RK45", args=(u, )) x_init = sol["y"][:, -1] add_to_data(i + 1, x_init) time_interp = np.linspace(0, final_time, nb_shooting + 1) return time_interp, markers, X, U
states, controls = Data.get_data(ocp, sol["x"]) q = states["q"] qdot = states["q_dot"] mus = controls["muscles"] if use_residual_torque: tau = controls["tau"] n_q = ocp.nlp[0].model.nbQ() n_mark = ocp.nlp[0].model.nbMarkers() n_frames = q.shape[1] markers = np.ndarray((3, n_mark, q.shape[1])) symbolic_states = MX.sym("x", n_q, 1) markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_states) for i in range(n_frames): markers[:, :, i] = markers_func(q[:, i]) plt.figure("Markers") for i in range(markers.shape[1]): plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers_ref[:, i, :].T, "k") plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers[:, i, :].T, "r--") # --- Plot --- # plt.show()
def main(): """ Generate random data, then create a tracking problem, and finally solve it and plot some relevant information """ # Define the problem biorbd_model = biorbd.Model("arm26.bioMod") final_time = 2 n_shooting_points = 29 use_residual_torque = True # Generate random data to fit t, markers_ref, x_ref, muscle_activations_ref = generate_data( biorbd_model, final_time, n_shooting_points, use_residual_torque=use_residual_torque) # Track these data biorbd_model = biorbd.Model( "arm26.bioMod" ) # To allow for non free variable, the model must be reloaded ocp = prepare_ocp( biorbd_model, final_time, n_shooting_points, markers_ref, muscle_activations_ref, x_ref[:biorbd_model.nbQ(), :], kin_data_to_track="q", use_residual_torque=use_residual_torque, ) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show the results --- # muscle_activations_ref = np.append(muscle_activations_ref, muscle_activations_ref[-1:, :], axis=0) q = sol.states["q"] qdot = sol.states["qdot"] mus = sol.controls["muscles"] if use_residual_torque: tau = sol.controls["tau"] n_q = ocp.nlp[0].model.nbQ() n_mark = ocp.nlp[0].model.nbMarkers() n_frames = q.shape[1] markers = np.ndarray((3, n_mark, q.shape[1])) symbolic_states = MX.sym("x", n_q, 1) markers_func = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_states) for i in range(n_frames): markers[:, :, i] = markers_func(q[:, i]) plt.figure("Markers") for i in range(markers.shape[1]): plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers_ref[:, i, :].T, "k") plt.plot(np.linspace(0, 2, n_shooting_points + 1), markers[:, i, :].T, "r--") # --- Plot --- # plt.show()
def main(): """ Firstly, it solves the getting_started/pendulum.py example. Afterward, it gets the marker positions and joint torque from the solution and uses them to track. It then creates and solves this ocp and show the results """ biorbd_path = str(EXAMPLES_FOLDER) + "/getting_started/pendulum.bioMod" biorbd_model = biorbd.Model(biorbd_path) final_time = 3 n_shooting = 20 ocp_to_track = data_to_track.prepare_ocp(biorbd_model_path=biorbd_path, final_time=3, n_shooting=19) sol = ocp_to_track.solve() q, qdot, tau = sol.states["q"], sol.states["qdot"], sol.controls["tau"] n_q = n_qdot = n_tau = biorbd_model.nbQ() n_marker = biorbd_model.nbMarkers() x = np.concatenate((q, qdot)) u = tau symbolic_states = MX.sym("q", n_q, 1) symbolic_controls = MX.sym("u", n_tau, 1) markers_fun = biorbd.to_casadi_func("ForwardKin", biorbd_model.markers, symbolic_states) markers_ref = np.zeros((3, n_marker, n_shooting + 1)) for i in range(n_shooting): markers_ref[:, :, i] = markers_fun(x[:n_q, i]) tau_ref = tau ocp = prepare_ocp( biorbd_model, final_time=final_time, n_shooting=n_shooting, markers_ref=markers_ref, tau_ref=tau_ref, ) # --- plot markers position --- # title_markers = ["x", "y", "z"] marker_color = ["tab:red", "tab:orange"] ocp.add_plot( "Markers plot coordinates", update_function=lambda x, u, p: get_markers_pos( x, 0, markers_fun, n_q), linestyle=".-", plot_type=PlotType.STEP, color=marker_color[0], ) ocp.add_plot( "Markers plot coordinates", update_function=lambda x, u, p: get_markers_pos( x, 1, markers_fun, n_q), linestyle=".-", plot_type=PlotType.STEP, color=marker_color[1], ) ocp.add_plot( "Markers plot coordinates", update_function=lambda x, u, p: markers_ref[:, 0, :], plot_type=PlotType.PLOT, color=marker_color[0], legend=title_markers, ) ocp.add_plot( "Markers plot coordinates", update_function=lambda x, u, p: markers_ref[:, 1, :], plot_type=PlotType.PLOT, color=marker_color[1], legend=title_markers, ) # --- Solve the program --- # sol = ocp.solve(show_online_optim=True) # --- Show results --- # sol.animate()