def online_muscle_torque(ocp: OptimalControlProgram): nlp = ocp.nlp[0] states = MX.sym("x", nlp.nx, 1) controls = MX.sym("u", nlp.nu, 1) parameters = MX.sym("u", nlp.np, 1) 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 = nlp.model.stateSet() muscles_activations = controls[nlp.shape["tau"]:] 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() muscle_tau_func = Function("muscle_tau", [states, controls, parameters], [muscles_tau]).expand() def muscle_tau_callback(s, c, p): return muscle_tau_func(s, c, p) ocp.add_plot( "muscle_torque", muscle_tau_callback, plot_type=PlotType.STEP, )
def configure_dynamics_function(ocp, nlp, dyn_func, **extra_params): """ Configure the forward dynamics Parameters ---------- ocp: OptimalControlProgram A reference to the ocp nlp: NonLinearProgram A reference to the phase dyn_func: Callable[states, controls, param] The function to get the derivative of the states """ mx_symbolic_states = MX.sym("x", nlp.states.shape, 1) mx_symbolic_controls = MX.sym("u", nlp.controls.shape, 1) nlp.parameters = ocp.v.parameters_in_list mx_symbolic_params = MX.sym("p", nlp.parameters.shape, 1) dynamics = dyn_func(mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params, nlp, **extra_params) if isinstance(dynamics, (list, tuple)): dynamics = vertcat(*dynamics) nlp.dynamics_func = Function( "ForwardDyn", [mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params], [dynamics], ["x", "u", "p"], ["xdot"], ).expand()
def configure_forward_dyn_func(ocp, nlp, dyn_func): nlp.nx = nlp.x.rows() nlp.nu = nlp.u.rows() MX_symbolic_states = MX.sym("x", nlp.nx, 1) MX_symbolic_controls = MX.sym("u", nlp.nu, 1) symbolic_params = nlp.CX() nlp.parameters_to_optimize = ocp.param_to_optimize for key in nlp.parameters_to_optimize: symbolic_params = vertcat(symbolic_params, nlp.parameters_to_optimize[key]["cx"]) nlp.p = symbolic_params nlp.np = symbolic_params.rows() MX_symbolic_params = MX.sym("p", nlp.np, 1) dynamics = dyn_func(MX_symbolic_states, MX_symbolic_controls, MX_symbolic_params, nlp) if isinstance(dynamics, (list, tuple)): dynamics = vertcat(*dynamics) nlp.dynamics_func = Function( "ForwardDyn", [MX_symbolic_states, MX_symbolic_controls, MX_symbolic_params], [dynamics], ["x", "u", "p"], ["xdot"], ).expand()
def __init__(self, dt=0.01, I=np.diag(np.array([2, 2, 4])), kd=1, k=1, L=0.3, b=1, m=1, g=9.81, Dx=12, Du=4): self.I = I #inertia self.I_inv = np.linalg.inv(self.I) self.kd = kd #friction self.k = k #motor constant self.L = L # distance between center and motor self.b = b # drag coefficient self.m = m # mass self.g = g self.Dx = Dx self.Du = Du self.dt = dt self.gravity = np.array([0, 0, -self.g]) self.x = MX.sym('x', Dx) self.u = MX.sym('u', Du) #initialise self.def_step_func(self.x, self.u) self.def_jacobian()
def configure_forward_dyn_func(ocp, nlp, dyn_func): nlp["nx"] = nlp["x"].rows() nlp["nu"] = nlp["u"].rows() MX_symbolic_states = MX.sym("x", nlp["nx"], 1) MX_symbolic_controls = MX.sym("u", nlp["nu"], 1) symbolic_params = nlp["CX"]() nlp["parameters_to_optimize"] = ocp.param_to_optimize for key in nlp["parameters_to_optimize"]: symbolic_params = vertcat(symbolic_params, nlp["parameters_to_optimize"][key]["cx"]) nlp["p"] = symbolic_params nlp["np"] = symbolic_params.rows() MX_symbolic_params = MX.sym("p", nlp["np"], 1) dynamics = dyn_func(MX_symbolic_states, MX_symbolic_controls, MX_symbolic_params, nlp) if isinstance(dynamics, (list, tuple)): dynamics = vertcat(*dynamics) nlp["dynamics_func"] = Function( "ForwardDyn", [MX_symbolic_states, MX_symbolic_controls, MX_symbolic_params], [dynamics], ["x", "u", "p"], ["xdot"], ).expand()
def configure_contact(ocp, nlp, dyn_func): symbolic_states = MX.sym("x", nlp.nx, 1) symbolic_controls = MX.sym("u", nlp.nu, 1) symbolic_param = MX.sym("p", nlp.np, 1) nlp.contact_forces_func = Function( "contact_forces_func", [symbolic_states, symbolic_controls, symbolic_param], [dyn_func(symbolic_states, symbolic_controls, symbolic_param, nlp)], ["x", "u", "p"], ["contact_forces"], ).expand() all_contact_names = [] for elt in ocp.nlp: all_contact_names.extend( [name.to_string() for name in elt.model.contactNames() if name.to_string() not in all_contact_names] ) if "contact_forces" in nlp.mapping["plot"]: phase_mappings = nlp.mapping["plot"]["contact_forces"] else: contact_names_in_phase = [name.to_string() for name in nlp.model.contactNames()] phase_mappings = Mapping([i for i, c in enumerate(all_contact_names) if c in contact_names_in_phase]) nlp.plot["contact_forces"] = CustomPlot( nlp.contact_forces_func, axes_idx=phase_mappings, legend=all_contact_names )
def test_markers(brbd): m = brbd.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 brbd.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 = brbd.to_casadi_func("Compute_Markers", m.markers, q_sym) markers_velocity_func = brbd.to_casadi_func("Compute_MarkersVelocity", m.markersVelocity, q_sym, q_dot_sym) markers = np.array(markers_func(q)) markers_dot = np.array(markers_velocity_func(q, q_dot)) elif not brbd.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 test_blockdiag(self): # Test blockdiag with DM correct_res = DM([[1, 1, 0, 0, 0], [1, 1, 0, 0, 0], [0, 0, 1, 1, 1], [0, 0, 1, 1, 1], [0, 0, 1, 1, 1]]) a = DM.ones(2, 2) b = DM.ones(3, 3) res = blockdiag(a, b) self.assertTrue(is_equal(res, correct_res)) # MX and DM mix a = MX.sym('a', 2, 2) b = DM.ones(1, 1) correct_res = MX.zeros(3, 3) correct_res[:2, :2] = a correct_res[2:, 2:] = b res = blockdiag(a, b) self.assertTrue(is_equal(res, correct_res, 30)) # SX and DM mix a = SX.sym('a', 2, 2) b = DM.ones(1, 1) correct_res = SX.zeros(3, 3) correct_res[:2, :2] = a correct_res[2:, 2:] = b res = blockdiag(a, b) self.assertTrue(is_equal(res, correct_res, 30)) # SX and MX a = SX.sym('a', 2, 2) b = MX.sym('b', 2, 2) self.assertRaises(ValueError, blockdiag, a, b)
def muscle_excitations_and_torque_driven_with_contact(nlp): """ Names states (nlp.x) and controls (nlp.u) and gives size to (nlp.nx) and (nlp.nu). Works with torques and muscles. :param nlp: An OptimalControlProgram class. """ ProblemType.__configure_q_qdot(nlp, True, False) ProblemType.__configure_tau(nlp, False, True) ProblemType.__configure_muscles(nlp, True, True) u = MX() x = MX() for i in range(nlp["nbMuscle"]): u = vertcat(u, MX.sym(f"Muscle_{nlp['muscleNames']}_excitation")) x = vertcat(x, MX.sym(f"Muscle_{nlp['muscleNames']}_activation")) nlp["u"] = vertcat(nlp["u"], u) nlp["x"] = vertcat(nlp["x"], x) nlp["var_states"]["muscles"] = nlp["nbMuscle"] nlp["var_controls"]["muscles"] = nlp["nbMuscle"] ProblemType.__configure_forward_dyn_func( nlp, Dynamics. forward_dynamics_muscle_excitations_and_torque_driven_with_contact) ProblemType.__configure_contact( nlp, Dynamics. forces_from_forward_dynamics_muscle_excitations_and_torque_driven_with_contact )
def export_external_ode_model(): model_name = 'external_ode' # Declare model variables x = MX.sym('x', 2) u = MX.sym('u', 1) xDot = MX.sym('xDot', 2) cdll.LoadLibrary('./test_external_lib/build/libexternal_ode_casadi.so') f_ext = external('libexternal_ode_casadi', 'libexternal_ode_casadi.so', {'enable_fd': True}) f_expl = f_ext(x, u) f_impl = xDot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xDot model.u = u model.p = [] model.name = model_name return model
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 Function, 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 = Function( "Compute_CoM", [q_sym], [m.CoM(q_sym).to_mx()], ["q"], ["CoM"], ).expand() CoM_dot_func = Function( "Compute_CoM_dot", [q_sym, q_dot_sym], [m.CoMdot(q_sym, q_dot_sym).to_mx()], ["q", "q_dot"], ["CoM_dot"], ).expand() CoM_ddot_func = Function( "Compute_CoM_ddot", [q_sym, q_dot_sym, q_ddot_sym], [m.CoMddot(q_sym, q_dot_sym, q_ddot_sym).to_mx()], ["q", "q_dot", "q_ddot"], ["CoM_ddot"], ).expand() 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() 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 define_substitute(self, name, expr): if isinstance(expr, list): return [ self.define_substitute(name + str(l), e) for l, e in enumerate(expr) ] else: if name in self._substitutes: raise ValueError('Name %s already used for substitutes!' % (name)) symbol_name = self._add_label(name) if isinstance(expr, BSpline): self._splines_prim[name] = {'basis': expr.basis} coeffs = MX.sym(symbol_name, expr.coeffs.shape[0], 1) subst = BSpline(expr.basis, coeffs) self._substitutes[name] = [expr.coeffs, subst.coeffs] inp_sym, inp_num = [], [] for sym in symvar(expr.coeffs): inp_sym.append(sym) inp_num.append( DM(self._values[self.symbol_dict[sym.name()][1]])) fun = Function('eval', inp_sym, [expr.coeffs]) self._values[name] = fun(*inp_num) self.add_to_dict(coeffs, name) else: subst = MX.sym(symbol_name, expr.shape[0], expr.shape[1]) self._substitutes[name] = [expr, subst] self.add_to_dict(subst, name) return subst
def configure_dynamics_function(ocp, nlp, dyn_func): """ Configure the forward dynamics Parameters ---------- ocp: OptimalControlProgram A reference to the ocp nlp: NonLinearProgram A reference to the phase dyn_func: Callable[states, controls, param] The function to get the derivative of the states """ nlp.nx = nlp.x.rows() nlp.nu = nlp.u.rows() mx_symbolic_states = MX.sym("x", nlp.nx, 1) mx_symbolic_controls = MX.sym("u", nlp.nu, 1) nlp.parameters = ocp.v.parameters_in_list nlp.p = ocp.v.parameters.cx nlp.np = sum([p.size for p in nlp.parameters]) mx_symbolic_params = MX.sym("p", nlp.np, 1) dynamics = dyn_func(mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params, nlp) if isinstance(dynamics, (list, tuple)): dynamics = vertcat(*dynamics) nlp.dynamics_func = Function( "ForwardDyn", [mx_symbolic_states, mx_symbolic_controls, mx_symbolic_params], [dynamics], ["x", "u", "p"], ["xdot"], ).expand()
def make_model_consistent(model): x = model.x xdot = model.xdot u = model.u z = model.z p = model.p if isinstance(x, SX): is_SX = True elif isinstance(x, MX): is_SX = False else: raise Exception( "model.x must be casadi.SX or casadi.MX, got {}".format(type(x))) if is_empty(p): if is_SX: model.p = SX.sym('p', 0, 0) else: model.p = MX.sym('p', 0, 0) if is_empty(z): if is_SX: model.z = SX.sym('z', 0, 0) else: model.z = MX.sym('z', 0, 0) return model
def configure_q_qdot(nlp, as_states, as_controls): """ Configures common settings for torque driven problems with and without contacts. :param nlp: An OptimalControlProgram class. """ if nlp["q_mapping"] is None: nlp["q_mapping"] = BidirectionalMapping( Mapping(range(nlp["model"].nbQ())), Mapping(range(nlp["model"].nbQ())) ) if nlp["q_dot_mapping"] is None: nlp["q_dot_mapping"] = BidirectionalMapping( Mapping(range(nlp["model"].nbQdot())), Mapping(range(nlp["model"].nbQdot())) ) dof_names = nlp["model"].nameDof() q_mx = MX() q_dot_mx = MX() q = nlp["CX"]() q_dot = nlp["CX"]() for i in nlp["q_mapping"].reduce.map_idx: q = vertcat(q, nlp["CX"].sym("Q_" + dof_names[i].to_string(), 1, 1)) for i in nlp["q_dot_mapping"].reduce.map_idx: q_dot = vertcat(q_dot, nlp["CX"].sym("Qdot_" + dof_names[i].to_string(), 1, 1)) for i in nlp["q_mapping"].expand.map_idx: q_mx = vertcat(q_mx, MX.sym("Q_" + dof_names[i].to_string(), 1, 1)) for i in nlp["q_dot_mapping"].expand.map_idx: q_dot_mx = vertcat(q_dot_mx, MX.sym("Qdot_" + dof_names[i].to_string(), 1, 1)) nlp["nbQ"] = nlp["q_mapping"].reduce.len nlp["nbQdot"] = nlp["q_dot_mapping"].reduce.len legend_q = ["q_" + nlp["model"].nameDof()[idx].to_string() for idx in nlp["q_mapping"].reduce.map_idx] legend_qdot = ["qdot_" + nlp["model"].nameDof()[idx].to_string() for idx in nlp["q_dot_mapping"].reduce.map_idx] nlp["q"] = q_mx nlp["qdot"] = q_dot_mx if as_states: nlp["x"] = vertcat(nlp["x"], q, q_dot) nlp["var_states"]["q"] = nlp["nbQ"] nlp["var_states"]["q_dot"] = nlp["nbQdot"] nlp["plot"]["q"] = CustomPlot( lambda x, u, p: x[: nlp["nbQ"]], plot_type=PlotType.INTEGRATED, legend=legend_q ) nlp["plot"]["q_dot"] = CustomPlot( lambda x, u, p: x[nlp["nbQ"] : nlp["nbQ"] + nlp["nbQdot"]], plot_type=PlotType.INTEGRATED, legend=legend_qdot, ) if as_controls: nlp["u"] = vertcat(nlp["u"], q, q_dot) nlp["var_controls"]["q"] = nlp["nbQ"] nlp["var_controls"]["q_dot"] = nlp["nbQdot"] # Add plot if it happens nlp["nx"] = nlp["x"].rows() nlp["nu"] = nlp["u"].rows()
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 Function, 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 = Function( "ForwardDynamics", [q_sym, qdot_sym, tau_sym], [m.ForwardDynamics(q_sym, qdot_sym, tau_sym, f_ext).to_mx()], ["q_sym", "qdot_sym", "tau_sym"], ["qddot"], ).expand() 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() 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 xia_model_configuration(ocp, nlp): Problem.configure_q_qdot(nlp, True, False) Problem.configure_tau(nlp, False, True) Problem.configure_muscles(nlp, False, True) x = MX() for i in range(nlp["nbMuscle"]): x = vertcat( x, MX.sym(f"Muscle_{nlp['muscleNames']}_active_{nlp['phase_idx']}", 1, 1)) for i in range(nlp["nbMuscle"]): x = vertcat( x, MX.sym(f"Muscle_{nlp['muscleNames']}_fatigue_{nlp['phase_idx']}", 1, 1)) for i in range(nlp["nbMuscle"]): x = vertcat( x, MX.sym(f"Muscle_{nlp['muscleNames']}_resting_{nlp['phase_idx']}", 1, 1)) nlp["x"] = vertcat(nlp["x"], x) nlp["var_states"]["muscles_active"] = nlp["nbMuscle"] nlp["var_states"]["muscles_fatigue"] = nlp["nbMuscle"] nlp["var_states"]["muscles_resting"] = nlp["nbMuscle"] nlp["nx"] = nlp["x"].rows() nb_q_qdot = nlp["nbQ"] + nlp["nbQdot"] nlp["plot"]["muscles_active"] = CustomPlot( lambda x, u, p: x[nb_q_qdot:nb_q_qdot + nlp["nbMuscle"]], plot_type=PlotType.INTEGRATED, legend=nlp["muscleNames"], color="r", ylim=[0, 1], ) combine = "muscles_active" nlp["plot"]["muscles_fatigue"] = CustomPlot( lambda x, u, p: x[nb_q_qdot + nlp["nbMuscle"]:nb_q_qdot + 2 * nlp[ "nbMuscle"]], plot_type=PlotType.INTEGRATED, legend=nlp["muscleNames"], combine_to=combine, color="g", ylim=[0, 1], ) nlp["plot"]["muscles_resting"] = CustomPlot( lambda x, u, p: x[nb_q_qdot + 2 * nlp["nbMuscle"]:nb_q_qdot + 3 * nlp[ "nbMuscle"]], plot_type=PlotType.INTEGRATED, legend=nlp["muscleNames"], combine_to=combine, color="b", ylim=[0, 1], ) Problem.configure_forward_dyn_func(ocp, nlp, xia_model_dynamic)
def test_include_parameter_twice(self): # Test to avoid override aop = AbstractOptimizationProblem() p1 = MX.sym('p1', 3) p2 = MX.sym('p2', 5) aop.include_parameter(p1) aop.include_parameter(p2) self.assertTrue(is_equal(aop.p[:p1.numel()], p1)) self.assertTrue(is_equal(aop.p[p1.numel():], p2))
def test_include_variable_twice(self): # Test to avoid override aop = AbstractOptimizationProblem() x = MX.sym('x', 3) y = MX.sym('y', 5) aop.include_variable(x) aop.include_variable(y) self.assertTrue(is_equal(aop.x[:x.numel()], x)) self.assertTrue(is_equal(aop.x[x.numel():], y))
def test_multistep_reachability(before_test_onestep_reachability): """ """ p, _, gp, k_fb, _, L_mu, L_sigm, c_safety, a, b = before_test_onestep_reachability T = 3 n_u, n_s = np.shape(k_fb) u_0 = .2 * np.random.randn(n_u, 1) k_fb_0 = np.random.randn( T - 1, n_s * n_u) # np.zeros((T-1,n_s*n_u))# np.random.randn(T-1,n_s*n_u) k_ff = np.random.randn(T - 1, n_u) # k_fb_ctrl = np.zeros((n_u,n_s))#np.random.randn(n_u,n_s) u_0_cas = MX.sym("u_0", (n_u, 1)) k_fb_cas_0 = MX.sym("k_fb", (T - 1, n_u * n_s)) k_ff_cas = MX.sym("k_ff", (T - 1, n_u)) p_new_cas, q_new_cas, _ = reach_cas.multi_step_reachability( p, u_0, k_fb_cas_0, k_ff_cas, gp, L_mu, L_sigm, c_safety, a, b) f = Function("f", [u_0_cas, k_fb_cas_0, k_ff_cas], [p_new_cas, q_new_cas]) k_fb_0_cas = np.copy(k_fb_0) # np.copy(k_fb_0) for i in range(T - 1): k_fb_0_cas[i, None, :] = k_fb_0_cas[i, None, :] + cas_reshape( k_fb, (1, n_u * n_s)) p_all_cas, q_all_cas = f(u_0, k_fb_0_cas, k_ff) k_ff_all = np.vstack((u_0.T, k_ff)) k_fb_apply = array_of_vec_to_array_of_mat(k_fb_0, n_u, n_s) for i in range(T - 1): k_fb_apply[i, :, :] += k_fb _, _, p_all_num, q_all_num = reach_num.multistep_reachability( p, gp, k_fb_apply, k_ff_all, L_mu, L_sigm, None, c_safety, 0, a, b, None) assert np.allclose(p_all_cas, p_all_num, r_tol, a_tol), "Are the centers of the ellipsoids same?" assert np.allclose(q_all_cas[0, :], q_all_num[0, :, :].reshape( (-1, n_s * n_s)), r_tol, a_tol), "Are the first shape matrices the same?" assert np.allclose(q_all_cas[1, :], q_all_num[1, :, :].reshape( (-1, n_s * n_s)), r_tol, a_tol), "Are the second shape matrices the same?" # assert np.allclose(q_all_cas[1,:],q_all_num[1,:].reshape((-1,n_s*n_s))), "Are the second shape matrices the same?" assert np.allclose(q_all_cas[-1, :], q_all_num[-1, :, :].reshape( (-1, n_s * n_s)), r_tol, a_tol), "Are the last shape matrices the same?" assert np.allclose(q_all_cas, q_all_num.reshape((T, n_s * n_s)), r_tol, a_tol), "Are the shape matrices the same?"
def shift_knot1_fwd(cfs, basis, t_shift): if isinstance(cfs, (SX, MX)): cfs_sym = MX.sym('cfs', cfs.shape) t_shift_sym = MX.sym('t_shift') T = shiftfirstknot_T(basis, t_shift_sym) cfs2_sym = mtimes(T, cfs_sym) fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand() return fun(cfs, t_shift) else: T = shiftfirstknot_T(basis, t_shift) return T.dot(cfs)
def test_loss_sat(before_loss_sat): """ Does saturating cost function return same as Matlab implementation?""" m, v, W, z, L = before_loss_sat m_cas = MX.sym('m', (3, 1)) v_cas = MX.sym('v_cas', (3, 3)) L_sym = utils_cas.loss_sat(m_cas, v_cas, z, W) f = Function('loss_sat', [m_cas, v_cas], [L_sym]) L_out = f(m, v) assert np.allclose(L, L_out, atol=1e-3), "Are the losses the same?"
def construct_upd_z(self, problem=None, lineq_updz=True): if problem is not None: self.problem_upd_z = problem self._lineq_updz = lineq_updz return 0. # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: raise ValueError('For now, only equality constrained QP ' + 'z-updates are allowed!') x_i = struct_symMX(self.q_i_struct) x_j = struct_symMX(self.q_ij_struct) l_i = struct_symMX(self.q_i_struct) l_ij = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') par = struct_symMX(self.par_global_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t / T # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A(par.cat) b = b(par.cat) # build KKT system and solve it via schur complement method l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat) f = -(l + rho * x) G = -(1 / rho) * mtimes(A, A.T) h = b + (1 / rho) * mtimes(A, f) mu = solve(G, h) z = -(1 / rho) * (mtimes(A.T, mu) + f) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi + l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, buildtime = create_function('upd_z_' + str(self._index), inp, out, self.options) self.problem_upd_z = prob return buildtime
def force_func(biorbd_model, use_activation=False): qMX = MX.sym("qMX", biorbd_model.nbQ(), 1) dqMX = MX.sym("dqMX", biorbd_model.nbQ(), 1) aMX = MX.sym("aMX", biorbd_model.nbMuscles(), 1) uMX = MX.sym("uMX", biorbd_model.nbMuscles(), 1) return Function( "MuscleForce", [qMX, dqMX, aMX, uMX], [muscles_forces(qMX, dqMX, aMX, uMX, biorbd_model, use_activation=use_activation)], ["qMX", "dqMX", "aMX", "uMX"], ["Force"], ).expand()
def construct_upd_z(self, problem=None, lineq_updz=True): if problem is not None: self.problem_upd_z = problem self._lineq_updz = lineq_updz return 0. # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: raise ValueError('For now, only equality constrained QP ' + 'z-updates are allowed!') x_i = struct_symMX(self.q_i_struct) x_j = struct_symMX(self.q_ij_struct) l_i = struct_symMX(self.q_i_struct) l_ij = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') par = struct_symMX(self.par_global_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t/T # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A(par.cat) b = b(par.cat) # build KKT system and solve it via schur complement method l, x = vertcat(l_i.cat, l_ij.cat), vertcat(x_i.cat, x_j.cat) f = -(l + rho*x) G = -(1/rho)*mtimes(A, A.T) h = b + (1/rho)*mtimes(A, f) mu = solve(G, h) z = -(1/rho)*(mtimes(A.T, mu)+f) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, buildtime = create_function('upd_z_'+str(self._index), inp, out, self.options) self.problem_upd_z = prob return buildtime
def intg_builtin(self, f, X, U, P, Z): # A single CVODES step DT = MX.sym("DT") t = MX.sym("t") t0 = MX.sym("t0") res = f(x=X, u=U, p=P, t=t0+t*DT, z=Z) data = {'x': X, 'p': vertcat(U, DT, P, t0), 'z': Z, 't': t, 'ode': DT * res["ode"], 'quad': DT * res["quad"], 'alg': res["alg"]} options = dict(self.intg_options) if self.intg in ["collocation"]: options["number_of_finite_elements"] = 1 I = integrator('intg_cvodes', self.intg, data, options) res = I.call({'x0': X, 'p': vertcat(U, DT, P, t0)}) return Function('F', [X, U, t0, DT, P], [res["xf"], MX(), res["qf"]], ['x0', 'u', 't0', 'DT', 'p'], ['xf', 'poly_coeff','qf'])
def __configure_forward_dyn_func(nlp, dyn_func): nlp["nu"] = nlp["u"].rows() nlp["nx"] = nlp["x"].rows() symbolic_states = MX.sym("x", nlp["nx"], 1) symbolic_controls = MX.sym("u", nlp["nu"], 1) nlp["dynamics_func"] = Function( "ForwardDyn", [symbolic_states, symbolic_controls], [dyn_func(symbolic_states, symbolic_controls, nlp)], ["x", "u"], ["xdot"], ).expand() # .map(nlp["ns"], "thread", 2)
def configure_contact(ocp, nlp, dyn_func: Callable): """ Configure the contact points Parameters ---------- ocp: OptimalControlProgram A reference to the ocp nlp: NonLinearProgram A reference to the phase dyn_func: Callable[states, controls, param] The function to get the values of contact forces from the dynamics """ symbolic_states = MX.sym("x", nlp.nx, 1) symbolic_controls = MX.sym("u", nlp.nu, 1) symbolic_param = MX.sym("p", nlp.np, 1) nlp.contact_forces_func = Function( "contact_forces_func", [symbolic_states, symbolic_controls, symbolic_param], [ dyn_func(symbolic_states, symbolic_controls, symbolic_param, nlp) ], ["x", "u", "p"], ["contact_forces"], ).expand() all_contact_names = [] for elt in ocp.nlp: all_contact_names.extend([ name.to_string() for name in elt.model.contactNames() if name.to_string() not in all_contact_names ]) if "contact_forces" in nlp.mapping["plot"]: phase_mappings = nlp.mapping["plot"]["contact_forces"] else: contact_names_in_phase = [ name.to_string() for name in nlp.model.contactNames() ] phase_mappings = Mapping([ i for i, c in enumerate(all_contact_names) if c in contact_names_in_phase ]) nlp.plot["contact_forces"] = CustomPlot(nlp.contact_forces_func, axes_idx=phase_mappings, legend=all_contact_names)
def test_forward_dynamics_with_external_forces(brbd): m = brbd.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 sv1 = np.array( ((11.1, 22.2, 33.3, 44.4, 55.5, 66.6), (11.1 * 2, 22.2 * 2, 33.3 * 2, 44.4 * 2, 55.5 * 2, 66.6 * 2)) ).T f_ext = brbd.to_spatial_vector(sv1) if brbd.currentLinearAlgebraBackend() == 1: 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) forward_dynamics = brbd.to_casadi_func("ForwardDynamics", m.ForwardDynamics, q_sym, qdot_sym, tau_sym, f_ext) qddot = forward_dynamics(q, qdot, tau) qddot = np.array(qddot)[:, 0] elif brbd.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 __configure_tau(nlp, as_states, as_controls): """ Configures common settings for torque driven problems with and without contacts. :param nlp: An OptimalControlProgram class. """ if nlp["tau_mapping"] is None: nlp["tau_mapping"] = BidirectionalMapping( Mapping(range(nlp["model"].nbGeneralizedTorque())), Mapping(range(nlp["model"].nbGeneralizedTorque()))) dof_names = nlp["model"].nameDof() u = MX() for i in nlp["tau_mapping"].reduce.map_idx: u = vertcat(u, MX.sym("Tau_" + dof_names[i].to_string(), 1, 1)) nlp["nbTau"] = nlp["tau_mapping"].reduce.len legend_tau = [ "tau_" + nlp["model"].nameDof()[idx].to_string() for idx in nlp["tau_mapping"].reduce.map_idx ] if as_states: nlp["x"] = u nlp["var_states"] = {"tau": nlp["nbTau"]} # Add plot if it happens if as_controls: nlp["u"] = u nlp["var_controls"] = {"tau": nlp["nbTau"]} nlp["plot"]["tau"] = CustomPlot(lambda x, u: u[:nlp["nbTau"]], plot_type=PlotType.STEP, legend=legend_tau)
def new_variable(self, name, dim, init, lb, ub): """ Add a new variable to the problem. Parameters ---------- name : string Variable name. dim : int Number of dimensions. init : array, shape=(dim,) Initial values. lb : array, shape=(dim,) Vector of lower bounds on variable values. ub : array, shape=(dim,) Vector of upper bounds on variable values. """ assert len(init) == len(lb) == len(ub) == dim var = MX.sym(name, dim) self.var_symbols.append(var) self.initvals += init self.var_index[name] = len(self.var_lbounds) self.var_lbounds += lb self.var_ubounds += ub return var
def _define_mx(self, name, size0, size1, dictionary, value=None): if value is None: value = np.zeros((size0, size1)) symbol_name = self._add_label(name) dictionary[name] = MX.sym(symbol_name, size0, size1) self._values[name] = value self.add_to_dict(dictionary[name], name) return dictionary[name]
def construct_upd_l(self, problem=None): if problem is not None: self.problem_upd_l = problem return 0. # create parameters z_ij = struct_symMX(self.q_ij_struct) l_ij = struct_symMX(self.q_ij_struct) x_j = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') rho = MX.sym('rho') inp = [x_j, z_ij, l_ij, t, T, rho] # update lambda l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat)) out = [l_ij_new] # create problem prob, buildtime = create_function('upd_l_'+str(self._index), inp, out, self.options) self.problem_upd_l = prob return buildtime
def construct_upd_res(self, problem=None): if problem is not None: self.problem_upd_res = problem return 0. # create parameters x_i = struct_symMX(self.q_i_struct) z_i = struct_symMX(self.q_i_struct) z_i_p = struct_symMX(self.q_i_struct) z_ij = struct_symMX(self.q_ij_struct) z_ij_p = struct_symMX(self.q_ij_struct) x_j = struct_symMX(self.q_ij_struct) t = MX.sym('t') T = MX.sym('T') t0 = t/T rho = MX.sym('rho') inp = [x_i, z_i, z_i_p, z_ij, z_ij_p, x_j, t, T, rho] # put symbols in MX structs (necessary for transformation) x_i = self.q_i_struct(x_i) z_i = self.q_i_struct(z_i) z_i_p = self.q_i_struct(z_i_p) z_ij = self.q_ij_struct(z_ij) z_ij_p = self.q_ij_struct(z_ij_p) x_j = self.q_ij_struct(x_j) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, z_i_p], tf, self.q_i) self._transform_spline([x_j, z_ij, z_ij_p], tf, self.q_ij) # compute residuals pr = mtimes((x_i.cat-z_i.cat).T, (x_i.cat-z_i.cat)) pr += mtimes((x_j.cat-z_ij.cat).T, (x_j.cat-z_ij.cat)) dr = rho*mtimes((z_i.cat-z_i_p.cat).T, (z_i.cat-z_i_p.cat)) dr += rho*mtimes((z_ij.cat-z_ij_p.cat).T, (z_ij.cat-z_ij_p.cat)) cr = rho*pr + dr out = [pr, dr, cr] # create problem prob, buildtime = create_function('upd_res_'+str(self._index), inp, out, self.options) self.problem_upd_res = prob return buildtime