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)