def _setup_controller(robot):
"""Demonstrates how to create a locomotion controller."""
desired_speed = (0, 0)
desired_twisting_speed = 0
gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
robot,
stance_duration=_STANCE_DURATION_SECONDS,
duty_factor=_DUTY_FACTOR,
initial_leg_phase=_INIT_PHASE_FULL_CYCLE,
initial_leg_state=_INIT_LEG_STATE)
state_estimator = com_velocity_estimator.COMVelocityEstimator(
robot, window_size=1)
sw_controller = raibert_swing_leg_controller.RaibertSwingLegController(
robot,
gait_generator,
state_estimator,
desired_speed=desired_speed,
desired_twisting_speed=desired_twisting_speed,
desired_height=robot.MPC_BODY_HEIGHT,
foot_clearance=0.01)
st_controller = torque_stance_leg_controller.TorqueStanceLegController(
robot,
gait_generator,
state_estimator,
desired_speed=desired_speed,
desired_twisting_speed=desired_twisting_speed,
desired_body_height=robot.MPC_BODY_HEIGHT,
body_mass=robot.MPC_BODY_MASS,
body_inertia=robot.MPC_BODY_INERTIA)
controller = locomotion_controller.LocomotionController(
robot=robot,
gait_generator=gait_generator,
state_estimator=state_estimator,
swing_leg_controller=sw_controller,
stance_leg_controller=st_controller,
clock=robot.GetTimeSinceReset)
return controller
def _setup_controller(self,
robot,
desired_speed=(0.0, 0.0),
desired_twisting_speed=0.0):
""" Build the MPC controller. """
gait_generator = openloop_gait_generator.OpenloopGaitGenerator(
robot,
stance_duration=self._constants.STANCE_DURATION_SECONDS,
duty_factor=self._constants.DUTY_FACTOR,
initial_leg_phase=self._constants.INIT_PHASE_FULL_CYCLE,
initial_leg_state=self._constants.INIT_LEG_STATE)
state_estimator = com_velocity_estimator.COMVelocityEstimator(
robot, window_size=20)
sw_controller = raibert_swing_leg_controller.RaibertSwingLegController(
robot,
gait_generator,
state_estimator,
desired_speed=desired_speed,
desired_twisting_speed=desired_twisting_speed,
desired_height=self._constants.MPC_BODY_HEIGHT,
foot_clearance=0.01)
st_controller = torque_stance_leg_controller.TorqueStanceLegController(
robot,
gait_generator,
state_estimator,
desired_speed=desired_speed,
desired_twisting_speed=desired_twisting_speed,
desired_body_height=self._constants.MPC_BODY_HEIGHT,
body_mass=self._constants.MPC_BODY_MASS,
body_inertia=self._constants.MPC_BODY_INERTIA)
controller = locomotion_controller.LocomotionController(
robot=robot,
gait_generator=gait_generator,
state_estimator=state_estimator,
swing_leg_controller=sw_controller,
stance_leg_controller=st_controller,
clock=self.get_time_since_reset)
return controller