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