Skip to content

Commit

Permalink
pyomeca#746 Issue Fixed
Browse files Browse the repository at this point in the history
  • Loading branch information
Alpha2Shahiri committed Nov 9, 2023
1 parent 3894866 commit 587d678
Show file tree
Hide file tree
Showing 9 changed files with 81 additions and 81 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -298,7 +298,7 @@ def expected_feedback_effort(controllers: list[PenaltyController], sensory_noise
controllers[0].controls.cx_start,
controllers[0].parameters.cx_start,
controllers[0].stochastic_variables.cx_start,
controllers[0].get_nlp,
controllers[0].nlp,
)
trace_k_sensor_k = cas.trace(k_matrix @ sensory_noise_matrix @ k_matrix.T)
e_fb = k_matrix @ ((hand_pos_velo - ref) + sensory_noise_magnitude)
Expand Down Expand Up @@ -329,7 +329,7 @@ def zero_acceleration(controller: PenaltyController, force_field_magnitude: floa
controller.controls.cx_start,
controller.parameters.cx_start,
controller.stochastic_variables.cx_start,
controller.get_nlp,
controller.nlp,
force_field_magnitude=force_field_magnitude,
with_noise=False,
)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -299,7 +299,7 @@ def expected_feedback_effort(controllers: list[PenaltyController]) -> cas.MX:
controllers[0].controls.cx_start,
controllers[0].parameters.cx_start,
controllers[0].stochastic_variables.cx_start,
controllers[0].get_nlp,
controllers[0].nlp,
)
e_fb = k_matrix @ ((estimated_ref - ref) + controllers[0].model.sensory_noise_magnitude)
jac_e_fb_x = cas.jacobian(e_fb, controllers[0].states.cx_start)
Expand Down
46 changes: 23 additions & 23 deletions bioptim/limits/constraints.py
Original file line number Diff line number Diff line change
Expand Up @@ -123,18 +123,18 @@ def _add_penalty_to_pool(self, controller: PenaltyController):

if self.penalty_type == PenaltyType.INTERNAL:
pool = (
controller.get_nlp.g_internal
if controller is not None and controller.get_nlp
controller.nlp.g_internal
if controller is not None and controller.nlp
else controller.ocp.g_internal
)
elif self.penalty_type == ConstraintType.IMPLICIT:
pool = (
controller.get_nlp.g_implicit
if controller is not None and controller.get_nlp
controller.nlp.g_implicit
if controller is not None and controller.nlp
else controller.ocp.g_implicit
)
elif self.penalty_type == PenaltyType.USER:
pool = controller.get_nlp.g if controller is not None and controller.get_nlp else controller.ocp.g
pool = controller.nlp.g if controller is not None and controller.nlp else controller.ocp.g
else:
raise ValueError(f"Invalid constraint type {self.penalty_type}.")
pool[self.list_index] = self
Expand Down Expand Up @@ -267,7 +267,7 @@ def non_slipping(
constraint.min_bound = np.array([0, 0])
constraint.max_bound = np.array([np.inf, np.inf])

contact = controller.get_nlp.contact_forces_func(
contact = controller.nlp.contact_forces_func(
controller.time.cx,
controller.states.cx_start,
controller.controls.cx_start,
Expand Down Expand Up @@ -338,7 +338,7 @@ def torque_max_from_q_and_qdot(
n_rows = value.shape[0] // 2
else:
if (
controller.get_nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE
controller.nlp.phase_dynamics == PhaseDynamics.ONE_PER_NODE
and not isinstance(constraint.rows, int)
and len(constraint.rows) == value.shape[0]
):
Expand Down Expand Up @@ -461,7 +461,7 @@ def tau_equals_inverse_dynamics(
tau = tau + passive_torque if with_passive_torque else tau
tau = tau + controller.model.ligament_joint_torque(q, qdot) if with_ligament else tau

if controller.get_nlp.external_forces:
if controller.nlp.external_forces:
raise NotImplementedError(
"This implicit constraint tau_equals_inverse_dynamics is not implemented yet with external forces"
)
Expand Down Expand Up @@ -554,7 +554,7 @@ def tau_from_muscle_equal_inverse_dynamics(
muscle_tau = muscle_tau + controller.model.ligament_joint_torque(q, qdot) if with_ligament else muscle_tau
qddot = controller.states["qddot"].mx if "qddot" in controller.states else controller.controls["qddot"].mx

if controller.get_nlp.external_forces:
if controller.nlp.external_forces:
raise NotImplementedError(
"This implicit constraint tau_from_muscle_equal_inverse_dynamics is not implemented yet with external forces"
)
Expand Down Expand Up @@ -591,7 +591,7 @@ def implicit_soft_contact_forces(_: Constraint, controller: PenaltyController, *
force_idx.append(4 + (6 * i_sc))
force_idx.append(5 + (6 * i_sc))

soft_contact_all = controller.get_nlp.soft_contact_forces_func(
soft_contact_all = controller.nlp.soft_contact_forces_func(
controller.states.mx, controller.controls.mx, controller.parameters.mx
)
soft_contact_force = soft_contact_all[force_idx]
Expand All @@ -615,7 +615,7 @@ def stochastic_covariance_matrix_continuity_implicit(
"""
# TODO: Charbie -> This is only True for x=[q, qdot], u=[tau] (have to think on how to generalize it)

if not controller.get_nlp.is_stochastic:
if not controller.nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")

if "cholesky_cov" in controller.stochastic_variables.keys():
Expand Down Expand Up @@ -650,7 +650,7 @@ def stochastic_covariance_matrix_continuity_implicit(
cov_implicit_deffect = cov_next - cov_matrix

penalty.expand = (
controller.get_nlp.dynamics_type.expand_dynamics
controller.nlp.dynamics_type.expand_dynamics
) # TODO: Charbie -> should this be always true?
penalty.explicit_derivative = True
penalty.multi_thread = True
Expand All @@ -667,7 +667,7 @@ def stochastic_df_dx_implicit(
This function constrains the stochastic matrix A to its actual value which is
A = df/dx
"""
if not controller.get_nlp.is_stochastic:
if not controller.nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")

dt = controller.tf / controller.ns
Expand Down Expand Up @@ -746,10 +746,10 @@ def stochastic_helper_matrix_collocation(
and G = collocation slope constraints (defects).
"""

if not controller.get_nlp.is_stochastic:
if not controller.nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")

polynomial_degree = controller.get_nlp.ode_solver.polynomial_degree
polynomial_degree = controller.nlp.ode_solver.polynomial_degree
Mc, _ = ConstraintFunction.Functions.collocation_jacobians(
penalty,
controller,
Expand Down Expand Up @@ -790,10 +790,10 @@ def stochastic_covariance_matrix_continuity_collocation(
"""
# TODO: Charbie -> This is only True for x=[q, qdot], u=[tau] (have to think on how to generalize it)

if not controller.get_nlp.is_stochastic:
if not controller.nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")

polynomial_degree = controller.get_nlp.ode_solver.polynomial_degree
polynomial_degree = controller.nlp.ode_solver.polynomial_degree
_, Pf = ConstraintFunction.Functions.collocation_jacobians(
penalty,
controller,
Expand Down Expand Up @@ -847,7 +847,7 @@ def stochastic_mean_sensory_input_equals_reference(
controls=controller.controls.cx_start,
parameters=controller.parameters.cx_start,
stochastic_variables=controller.stochastic_variables.cx_start,
nlp=controller.get_nlp,
nlp=controller.nlp,
)
return sensory_input - ref

Expand Down Expand Up @@ -1176,18 +1176,18 @@ def _add_penalty_to_pool(self, controller: PenaltyController):

if self.penalty_type == PenaltyType.INTERNAL:
pool = (
controller.get_nlp.g_internal
if controller is not None and controller.get_nlp
controller.nlp.g_internal
if controller is not None and controller.nlp
else controller.ocp.g_internal
)
elif self.penalty_type == ConstraintType.IMPLICIT:
pool = (
controller.get_nlp.g_implicit
if controller is not None and controller.get_nlp
controller.nlp.g_implicit
if controller is not None and controller.nlp
else controller.ocp.g_implicit
)
elif self.penalty_type == PenaltyType.USER:
pool = controller.get_nlp.g if controller is not None and controller.get_nlp else controller.ocp.g
pool = controller.nlp.g if controller is not None and controller.nlp else controller.ocp.g
else:
raise ValueError(f"Invalid constraint type {self.penalty_type}.")
pool[self.list_index] = self
Expand Down
10 changes: 5 additions & 5 deletions bioptim/limits/multinode_penalty.py
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ def _add_penalty_to_pool(self, controller: list[PenaltyController, PenaltyContro
)

ocp = controller[0].ocp
nlp = controller[0].get_nlp
nlp = controller[0].nlp
pool = self._get_pool_to_add_penalty(ocp, nlp)
pool[self.list_index] = self

Expand Down Expand Up @@ -378,7 +378,7 @@ def stochastic_helper_matrix_explicit(
controllers: list[PenaltyController, PenaltyController]
"""

if not controllers[0].get_nlp.is_stochastic:
if not controllers[0].nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")
if controllers[0].phase_idx != controllers[1].phase_idx:
raise RuntimeError("For this constraint to make sens, the two nodes must belong to the same phase.")
Expand Down Expand Up @@ -447,7 +447,7 @@ def stochastic_helper_matrix_implicit(
controllers: list[PenaltyController, PenaltyController]
The penalty node elements
"""
if not controllers[0].get_nlp.is_stochastic:
if not controllers[0].nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")
if controllers[0].phase_idx != controllers[1].phase_idx:
raise RuntimeError("For this constraint to make sens, the two nodes must belong to the same phase.")
Expand Down Expand Up @@ -481,7 +481,7 @@ def stochastic_covariance_matrix_continuity_implicit(
"""
# TODO: Charbie -> This is only True for x=[q, qdot], u=[tau] (have to think on how to generalize it)

if not controllers[0].get_nlp.is_stochastic:
if not controllers[0].nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")

cov_matrix = StochasticBioModel.reshape_to_matrix(
Expand Down Expand Up @@ -522,7 +522,7 @@ def stochastic_df_dw_implicit(
"""
# TODO: Charbie -> This is only True for x=[q, qdot], u=[tau] (have to think on how to generalize it)

if not controllers[0].get_nlp.is_stochastic:
if not controllers[0].nlp.is_stochastic:
raise RuntimeError("This function is only valid for stochastic problems")

dt = controllers[0].tf / controllers[0].ns
Expand Down
12 changes: 6 additions & 6 deletions bioptim/limits/objective_functions.py
Original file line number Diff line number Diff line change
Expand Up @@ -83,12 +83,12 @@ def _add_penalty_to_pool(self, controller: PenaltyController):

if self.penalty_type == PenaltyType.INTERNAL:
pool = (
controller.get_nlp.J_internal
if controller is not None and controller.get_nlp
controller.nlp.J_internal
if controller is not None and controller.nlp
else controller.ocp.J_internal
)
elif self.penalty_type == PenaltyType.USER:
pool = controller.get_nlp.J if controller is not None and controller.get_nlp else controller.ocp.J
pool = controller.nlp.J if controller is not None and controller.nlp else controller.ocp.J
else:
raise ValueError(f"Invalid objective type {self.penalty_type}.")
pool[self.list_index] = self
Expand Down Expand Up @@ -492,12 +492,12 @@ def _add_penalty_to_pool(self, controller: PenaltyController):

if self.penalty_type == PenaltyType.INTERNAL:
pool = (
controller.get_nlp.J_internal
if controller is not None and controller.get_nlp
controller.nlp.J_internal
if controller is not None and controller.nlp
else controller.ocp.J_internal
)
elif self.penalty_type == PenaltyType.USER:
pool = controller.get_nlp.J if controller is not None and controller.get_nlp else controller.ocp.J
pool = controller.nlp.J if controller is not None and controller.nlp else controller.ocp.J
else:
raise ValueError(f"Invalid objective type {self.penalty_type}.")
pool[self.list_index] = self
Expand Down
32 changes: 16 additions & 16 deletions bioptim/limits/penalty.py
Original file line number Diff line number Diff line change
Expand Up @@ -87,12 +87,12 @@ def minimize_controls(penalty: PenaltyOption, controller: PenaltyController, key
"""

penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic
if key in controller.get_nlp.variable_mappings:
target_mapping = controller.get_nlp.variable_mappings[key]
if key in controller.nlp.variable_mappings:
target_mapping = controller.nlp.variable_mappings[key]
else:
target_mapping = BiMapping(
to_first=list(range(controller.get_nlp.controls[key].cx_start.shape[0])),
to_second=list(range(controller.get_nlp.controls[key].cx_start.shape[0])),
to_first=list(range(controller.nlp.controls[key].cx_start.shape[0])),
to_second=list(range(controller.nlp.controls[key].cx_start.shape[0])),
) # TODO: why if condition, target_mapping not used (Pariterre?)

if penalty.integration_rule == QuadratureRule.RECTANGLE_LEFT:
Expand Down Expand Up @@ -159,12 +159,12 @@ def stochastic_minimize_variables(penalty: PenaltyOption, controller: PenaltyCon
penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic
penalty.multi_thread = True if penalty.multi_thread is None else penalty.multi_thread

if key in controller.get_nlp.variable_mappings:
target_mapping = controller.get_nlp.variable_mappings[key]
if key in controller.nlp.variable_mappings:
target_mapping = controller.nlp.variable_mappings[key]
else:
target_mapping = BiMapping(
to_first=list(range(controller.get_nlp.controls[key].cx_start.shape[0])),
to_second=list(range(controller.get_nlp.controls[key].cx_start.shape[0])),
to_first=list(range(controller.nlp.controls[key].cx_start.shape[0])),
to_second=list(range(controller.nlp.controls[key].cx_start.shape[0])),
)

return controller.stochastic_variables[key].cx_start
Expand Down Expand Up @@ -223,7 +223,7 @@ def stochastic_minimize_expected_feedback_efforts(penalty: PenaltyOption, contro
controls=controller.controls.cx_start,
parameters=controller.parameters.cx_start,
stochastic_variables=controller.stochastic_variables.cx_start,
nlp=controller.get_nlp,
nlp=controller.nlp,
)

e_fb = k_matrix @ ((ee - ref) + controller.model.sensory_noise_magnitude)
Expand Down Expand Up @@ -805,13 +805,13 @@ def minimize_contact_forces(
penalty.cols should not be defined if contact_index is defined
"""

if controller.get_nlp.contact_forces_func is None:
if controller.nlp.contact_forces_func is None:
raise RuntimeError("minimize_contact_forces requires a contact dynamics")

PenaltyFunctionAbstract.set_axes_rows(penalty, contact_index)
penalty.quadratic = True if penalty.quadratic is None else penalty.quadratic

contact_force = controller.get_nlp.contact_forces_func(
contact_force = controller.nlp.contact_forces_func(
controller.time.cx,
controller.states.cx_start,
controller.controls.cx_start,
Expand Down Expand Up @@ -840,7 +840,7 @@ def minimize_soft_contact_forces(
penalty.cols should not be defined if contact_index is defined
"""

if controller.get_nlp.soft_contact_forces_func is None:
if controller.nlp.soft_contact_forces_func is None:
raise RuntimeError("minimize_contact_forces requires a soft contact dynamics")

PenaltyFunctionAbstract.set_axes_rows(penalty, contact_index)
Expand All @@ -851,7 +851,7 @@ def minimize_soft_contact_forces(
force_idx.append(3 + (6 * i_sc))
force_idx.append(4 + (6 * i_sc))
force_idx.append(5 + (6 * i_sc))
soft_contact_force = controller.get_nlp.soft_contact_forces_func(
soft_contact_force = controller.nlp.soft_contact_forces_func(
controller.time.cx, controller.states.cx_start, controller.controls.cx_start, controller.parameters.cx
)
return soft_contact_force[force_idx]
Expand Down Expand Up @@ -1127,19 +1127,19 @@ def state_continuity(penalty: PenaltyOption, controller: PenaltyController | lis
if isinstance(penalty.node, (list, tuple)) and len(penalty.node) != 1:
raise RuntimeError("continuity should be called one node at a time")

penalty.expand = controller.get_nlp.dynamics_type.expand_continuity
penalty.expand = controller.nlp.dynamics_type.expand_continuity

if (
len(penalty.node_idx) > 1
and not controller.get_nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE
and not controller.nlp.phase_dynamics == PhaseDynamics.SHARED_DURING_THE_PHASE
):
raise NotImplementedError(
f"Length of node index superior to 1 is not implemented yet,"
f" actual length {len(penalty.node_idx[0])} "
)

continuity = controller.states.cx_end
if controller.get_nlp.ode_solver.is_direct_collocation:
if controller.nlp.ode_solver.is_direct_collocation:
cx = horzcat(*([controller.states.cx_start] + controller.states.cx_intermediates_list))
continuity -= controller.integrate(
x0=cx, u=u, p=controller.parameters.cx, s=controller.stochastic_variables.cx_start
Expand Down
2 changes: 1 addition & 1 deletion bioptim/limits/penalty_controller.py
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ def ocp(self):
return self._ocp

@property
def get_nlp(self):
def nlp(self):
"""
This method returns the underlying nlp. Please note that acting directly with the nlp is not want you should do.
Unless you see no way to access what you need otherwise, we strongly suggest that you use the normal path
Expand Down
Loading

0 comments on commit 587d678

Please sign in to comment.