def __init__(
self,
robot: Any,
gait_generator: Any,
state_estimator: Any,
desired_speed: Tuple[float, float] = (0, 0),
desired_twisting_speed: float = 0,
desired_body_height: float = 0.45,
body_mass: float = 220 / 9.8,
body_inertia: Tuple[float, float, float, float, float, float, float,
float, float] = (0.07335, 0, 0, 0, 0.25068, 0, 0, 0,
0.25447),
num_legs: int = 4,
friction_coeffs: Sequence[float] = (0.45, 0.45, 0.45, 0.45),
qp_solver = convex_mpc.QPOASES
):
"""Initializes the class.
Tracks the desired position/velocity of the robot by computing proper joint
torques using MPC module.
Args:
robot: A robot instance.
gait_generator: Used to query the locomotion phase and leg states.
state_estimator: Estimate the robot states (e.g. CoM velocity).
desired_speed: desired CoM speed in x-y plane.
desired_twisting_speed: desired CoM rotating speed in z direction.
desired_body_height: The standing height of the robot.
body_mass: The total mass of the robot.
body_inertia: The inertia matrix in the body principle frame. We assume
the body principle coordinate frame has x-forward and z-up.
num_legs: The number of legs used for force planning.
friction_coeffs: The friction coeffs on the contact surfaces.
"""
self._robot = robot
self._gait_generator = gait_generator
self._state_estimator = state_estimator
self.desired_speed = desired_speed
self.desired_twisting_speed = desired_twisting_speed
self._desired_body_height = desired_body_height
self._body_mass = body_mass
self._num_legs = num_legs
self._friction_coeffs = np.array(friction_coeffs)
body_inertia_list = list(body_inertia)
weights_list = list(_MPC_WEIGHTS)
self._cpp_mpc = convex_mpc.ConvexMpc(
body_mass,
body_inertia_list,
self._num_legs,
_PLANNING_HORIZON_STEPS,
_PLANNING_TIMESTEP,
weights_list,
1e-5,
qp_solver
)
def __init__(
self,
robot: Any,
gait_generator: Any,
state_estimator: Any,
desired_speed: Tuple[float] = (0, 0),
desired_twisting_speed: float = 0,
desired_roll_pitch: Tuple[float] = (0, 0),
desired_body_height: float = 0.45,
body_mass: float = 220 / 9.8,
body_inertia: Tuple[float] = (0.183375, 0, 0, 0, 0.6267, 0, 0, 0,
0.636175),
num_legs: int = 4,
friction_coeffs: Sequence[float] = (0.5, 0.5, 0.5, 0.5),
qp_weights: Sequence[float] = _MPC_WEIGHTS,
planning_horizon: int = _PLANNING_HORIZON_STEPS,
planning_timestep: int = _PLANNING_TIMESTEP,
):
"""Initializes the class.
Tracks the desired position/velocity of the robot by computing proper joint
torques using MPC module.
Args:
robot: A robot instance.
gait_generator: Used to query the locomotion phase and leg states.
state_estimator: Estimate the robot states (e.g. CoM velocity).
desired_speed: desired CoM speed in x-y plane.
desired_twisting_speed: desired CoM rotating speed in z direction.
desired_roll_pitch: desired CoM roll and pitch.
desired_body_height: The standing height of the robot.
body_mass: The total mass of the robot.
body_inertia: The inertia matrix in the body principle frame. We assume
the body principle coordinate frame has x-forward and z-up.
num_legs: The number of legs used for force planning.
friction_coeffs: The friction coeffs on the contact surfaces.
qp_weights: The weights used in solving the QP problem.
planning_horizon: Number of steps to roll-out in the QP formulation.
planning_timestep: Timestep between each step in the QP formulation.
"""
self._robot = robot
self._gait_generator = gait_generator
self._state_estimator = state_estimator
self._desired_speed = desired_speed
self._desired_twisting_speed = desired_twisting_speed
self._desired_roll_pitch = desired_roll_pitch
self._desired_body_height = desired_body_height
self._body_mass = body_mass
self._num_legs = num_legs
self._friction_coeffs = np.array(friction_coeffs)
self._qp_solver_fail = False
self._com_estimate_leg_indices = None
qp_solver = convex_mpc.QPOASES #convex_mpc.OSQP #
body_inertia_list = list(body_inertia)
weights_list = list(qp_weights)
self._mpc = convex_mpc.ConvexMpc(body_mass, body_inertia_list,
self._num_legs, planning_horizon,
planning_timestep, weights_list, 1e-6,
qp_solver)