diff --git a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py index 37d20b728b99051c961f037b59db82a76a78977d..b055ea2f2f970a69e85ee9e08c662be07342443f 100644 --- a/python/quadruped_reactive_walking/WB_MPC/ProblemData.py +++ b/python/quadruped_reactive_walking/WB_MPC/ProblemData.py @@ -4,6 +4,35 @@ import pinocchio as pin class problemDataAbstract: + """Parameters of an abstract optimal control problem + + Attributes: + dt (float): time step of the OCP + dt_wbc (float): time step of the low level integration + mpc_wbc_ratio (int): ratio between OCP and integration time steps + T (int): number of running nodes in the OCP + robot (RobotWrapper): wrapper containing data about the robot + q0 (vector): default configuration of the robot + model (Model): model of the robot + data (Data): data related to the model of the robot + collision_model (GeometryModel): collision mesh of the robot + visual_model (GeometryModel): visual mesh of the robot + frozen_names (list): names of frozen joints + frozen_idxs (list): IDs of frozen joints + nq (int): size of the configuration vector + nv (int): size of the velocity vector + nx (int): size of the state vector + ndx (int): size of the derivative of the state vector + nu (int): size of the control vector + ntau (int): size of the effort vector + v0 (vector): initial velocity vector + x0 (vector): initial state vector + u0 (vector): initial control vector + baumgarte_gains (array): Baumgarte gains for the contacts + xref (vector): reference state + uref (vector): reference control + """ + def __init__(self, param, frozen_names=[]): self.dt = param.dt_mpc # OCP dt self.dt_wbc = param.dt_wbc @@ -75,24 +104,45 @@ class problemDataAbstract: class ProblemData(problemDataAbstract): + """Parameters of the optimal control problem + Base is not fixed, with a base velocity tracking task + + Attributes: + useFixedBase (bool): whether the base is fixed + baseId (int): ID of the base frame + mu (float): friction coefficient + base_velocity_tracking_w (float): weight of the base velocity tracking task + ground_collision_w (float): weight of the ground collision task + friction_cone_w (float): weight of the friction cone constraint + impact_altitude_w (float): weight of the impact altitude constraint + impact_velocity_w (float): weight of the impact velocity constraint + control_bound_w (float): weight on the control bound contraint + control_reg_w (float): weight of the control regularization + state_reg_w (float): weight of the state regularization + terminal_velocity_w (float): weight on the terminal velocity value + force_reg_w (float): weight of the contact force regularization + xref (vector): reference state + uref (vector): reference control + """ + def __init__(self, param): super().__init__(param) self.useFixedBase = 0 - self.baseId = self.model.getFrameId("base_link") + self.mu = 0.7 # Cost function weights - # Cost function weights - self.mu = 0.7 self.base_velocity_tracking_w = 1e2 * 0 self.ground_collision_w = 1e3 * 0 + # Constraints weights self.friction_cone_w = 1e4 self.impact_altitude_w = 1e3 self.impact_velocity_w = 1e3 - self.control_bound_w = 1e3 * 0 + + # Regularization weights self.control_reg_w = 1e0 self.state_reg_w = np.array( [0] * 3 @@ -106,11 +156,31 @@ class ProblemData(problemDataAbstract): self.terminal_velocity_w = np.array([0] * self.nv + [1e4] * self.nv) self.force_reg_w = 1e0 + # Reference values self.xref = self.x0 self.uref = self.u0 class ProblemDataFull(problemDataAbstract): + """Parameters of the optimal control problem + Base is fixed, with a foot tracking task + + Attributes: + useFixedBase (bool): whether the base is fixed + mu (float): friction coefficient + foot_tracking_w (float): weight of the foot tracking task + friction_cone_w (float): weight of the friction cone constraint + control_bound_w (float): weight on the control bound contraint + control_reg_w (float): weight of the control regularization + state_reg_w (float): weight of the state regularization + terminal_velocity_w (float): weight on the terminal velocity value + q0_reduced (vector): reduced configuration vector (base is fixed) + v0_reduced (vector): reduced velocity vector (base is fixed) + x0_reduced (vector): reduced initial state + xref (vector): reference state + uref (vector): reference control + """ + def __init__(self, param): frozen_names = ["root_joint"] diff --git a/python/quadruped_reactive_walking/WB_MPC/Target.py b/python/quadruped_reactive_walking/WB_MPC/Target.py index 0b50f1c58e2c99aa099ff0ace20ff7a9fe84e2a6..040781b59f0b98b6eb368b4477b8fd3472641fea 100644 --- a/python/quadruped_reactive_walking/WB_MPC/Target.py +++ b/python/quadruped_reactive_walking/WB_MPC/Target.py @@ -12,7 +12,7 @@ class Target: Attributes: params (Params): object storing parameters about the control problem as a whole - dt_wbc (float): time step of the OCP + dt_wbc (float): time step of the low level integration linear_velocity (array): velocity target for the base linear velocity tracking task angular_velocity (array): velocity target for the base angular velocity tracking task velocity_task (list of arrays): targets for linear and angular velocities