RobotRunner Class Reference

#include <RobotRunner.h>

Inheritance diagram for RobotRunner:

Collaboration diagram for RobotRunner:

Public Member Functions
EIGEN_MAKE_ALIGNED_OPERATOR_NEW	RobotRunner (RobotController *, PeriodicTaskManager *, float, std::string)
void	init () override
void	run () override
void	cleanup () override
void	initializeStateEstimator (bool cheaterMode=false)
virtual	~RobotRunner ()
Public Member Functions inherited from PeriodicTask
	PeriodicTask (PeriodicTaskManager *taskManager, float period, std::string name)
void	start ()
void	stop ()
void	printStatus ()
void	clearMax ()
bool	isSlow ()
virtual	~PeriodicTask ()
float	getPeriod ()
float	getRuntime ()
float	getMaxPeriod ()
float	getMaxRuntime ()

Public Attributes
RobotController *	_robot_ctrl
GamepadCommand *	driverCommand
RobotType	robotType
VectorNavData *	vectorNavData
CheaterState< double > *	cheaterState
SpiData *	spiData
SpiCommand *	spiCommand
TiBoardCommand *	tiBoardCommand
TiBoardData *	tiBoardData
RobotControlParameters *	controlParameters
VisualizationData *	visualizationData
CheetahVisualization *	cheetahMainVisualization

Private Member Functions
void	setupStep ()
void	finalizeStep ()

Private Attributes
float	_ini_yaw
int	iter = 0
JPosInitializer< float > *	_jpos_initializer
Quadruped< float >	_quadruped
LegController< float > *	_legController = nullptr
StateEstimate< float >	_stateEstimate
StateEstimatorContainer< float > *	_stateEstimator
bool	_cheaterModeEnabled = false
DesiredStateCommand< float > *	_desiredStateCommand
lcm::LCM	_lcm
leg_control_command_lcmt	leg_control_command_lcm
state_estimator_lcmt	state_estimator_lcm
leg_control_data_lcmt	leg_control_data_lcm
FloatingBaseModel< float >	_model
u64	_iterations = 0

Detailed Description

Definition at line 30 of file RobotRunner.h.

Constructor & Destructor Documentation

RobotRunner::RobotRunner	(	RobotController *	robot_ctrl,
		PeriodicTaskManager *	manager,
		float	period,
		std::string	name
	)

Definition at line 15 of file RobotRunner.cpp.

References _robot_ctrl.

                                                      :
                         PeriodicTask(manager, period, name),
                         _lcm(getLcmUrl(255)) {

    _robot_ctrl = robot_ctrl;
}

RobotRunner::~RobotRunner ( )

virtual

Definition at line 189 of file RobotRunner.cpp.

References _jpos_initializer, _legController, and _stateEstimator.

                          {
  delete _legController;
  delete _stateEstimator;
  delete _jpos_initializer;
}

Member Function Documentation

void RobotRunner::cleanup ( )

overridevirtual

Implements PeriodicTask.

Definition at line 195 of file RobotRunner.cpp.

{}

void RobotRunner::finalizeStep ( )

private

Definition at line 158 of file RobotRunner.cpp.

References _iterations, _lcm, _legController, _stateEstimate, CHEETAH_3, leg_control_command_lcm, leg_control_data_lcm, MINI_CHEETAH, robotType, StateEstimate< T >::setLcm(), LegController< T >::setLcm(), spiCommand, state_estimator_lcm, tiBoardCommand, and LegController< T >::updateCommand().

                               {
  if (robotType == RobotType::MINI_CHEETAH) {
    _legController->updateCommand(spiCommand);
  } else if (robotType == RobotType::CHEETAH_3) {
    _legController->updateCommand(tiBoardCommand);
  } else {
    assert(false);
  }
  _legController->setLcm(&leg_control_data_lcm, &leg_control_command_lcm);
  _stateEstimate.setLcm(state_estimator_lcm);
  _lcm.publish("leg_control_command", &leg_control_command_lcm);
  _lcm.publish("leg_control_data", &leg_control_data_lcm);
  _lcm.publish("state_estimator", &state_estimator_lcm);
  _iterations++;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotRunner::init ( )

overridevirtual

Initializes the robot model, state estimator, leg controller, robot data, and any control logic specific data.

Implements PeriodicTask.

Definition at line 28 of file RobotRunner.cpp.

References RobotController::_controlParameters, RobotController::_driverCommand, _jpos_initializer, RobotController::_legController, _legController, RobotController::_model, _model, RobotController::_quadruped, _quadruped, _robot_ctrl, RobotController::_robotType, RobotController::_stateEstimate, _stateEstimate, RobotController::_stateEstimator, _stateEstimator, RobotController::_visualizationData, Quadruped< T >::buildModel(), cheaterState, controlParameters, LegController< T >::datas, driverCommand, RobotController::initializeController(), initializeStateEstimator(), MINI_CHEETAH, robotType, vectorNavData, and visualizationData.

                       {
  printf("[RobotRunner] initialize\n");

  // Build the appropriate Quadruped object
  if (robotType == RobotType::MINI_CHEETAH) {
    _quadruped = buildMiniCheetah<float>();
  } else {
    _quadruped = buildCheetah3<float>();
  }

  // Initialize the model and robot data
  _model = _quadruped.buildModel();
  _jpos_initializer = new JPosInitializer<float>(3.);
  
  // Always initialize the leg controller and state entimator
  _legController = new LegController<float>(_quadruped);
  _stateEstimator = new StateEstimatorContainer<float>(
      cheaterState, vectorNavData, _legController->datas,
      &_stateEstimate, controlParameters);
  initializeStateEstimator(false);

  // Controller initializations
  _robot_ctrl->_model = &_model;
  _robot_ctrl->_quadruped = &_quadruped;
  _robot_ctrl->_legController = _legController;
  _robot_ctrl->_stateEstimator = _stateEstimator;
  _robot_ctrl->_stateEstimate = &_stateEstimate;
  _robot_ctrl->_visualizationData= visualizationData;
  _robot_ctrl->_robotType = robotType;
  _robot_ctrl->_driverCommand = driverCommand;
  _robot_ctrl->_controlParameters = controlParameters;

  _robot_ctrl->initializeController();
}

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotRunner::initializeStateEstimator

(

bool

cheaterMode = false

)

Definition at line 174 of file RobotRunner.cpp.

References _stateEstimator, StateEstimatorContainer< T >::addEstimator(), StateEstimatorContainer< T >::removeAllEstimators(), and StateEstimatorContainer< T >::setContactPhase().

                                                           {
  _stateEstimator->removeAllEstimators();
  _stateEstimator->addEstimator<ContactEstimator<float>>();
  Vec4<float> contactDefault;
  contactDefault << 0.5, 0.5, 0.5, 0.5;
  _stateEstimator->setContactPhase(contactDefault);
  if (cheaterMode) {
    _stateEstimator->addEstimator<CheaterOrientationEstimator<float>>();
    _stateEstimator->addEstimator<CheaterPositionVelocityEstimator<float>>();
  } else {
    _stateEstimator->addEstimator<VectorNavOrientationEstimator<float>>();
    _stateEstimator->addEstimator<LinearKFPositionVelocityEstimator<float>>();
  }
}

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotRunner::run ( )

overridevirtual

Runs the overall robot control system by calling each of the major components to run each of their respective steps.

Implements PeriodicTask.

Definition at line 67 of file RobotRunner.cpp.

References _jpos_initializer, _legController, _robot_ctrl, _stateEstimate, _stateEstimator, cheetahMainVisualization, VisualizationData::clear(), LegController< T >::commands, LegController< T >::datas, finalizeStep(), JPosInitializer< T >::IsInitialized(), LegControllerCommand< T >::kdJoint, LegControllerCommand< T >::kpJoint, CheetahVisualization::p, StateEstimate< T >::position, CheetahVisualization::q, LegControllerData< T >::q, StateEstimatorContainer< T >::run(), RobotController::runController(), LegController< T >::setEnabled(), setupStep(), RobotController::updateVisualization(), and visualizationData.

                      {
  // Run the state estimator step
  _stateEstimator->run(cheetahMainVisualization);
  cheetahMainVisualization->p = _stateEstimate.position;
  visualizationData->clear();

  // Update the data from the robot
  setupStep();

  static int count_ini(0);
  ++count_ini;
  if (count_ini < 10) {
    _legController->setEnabled(false);
  } else if (20 < count_ini && count_ini < 30) {
    _legController->setEnabled(false);
  } else if (40 < count_ini && count_ini < 50) {
    _legController->setEnabled(false);
  } else {
    _legController->setEnabled(true);

    // Controller
    if (!_jpos_initializer->IsInitialized(_legController)) {
      Mat3<float> kpMat;
      Mat3<float> kdMat;
      kpMat << 5, 0, 0, 0, 5, 0, 0, 0, 5;
      kdMat << 0.1, 0, 0, 0, 0.1, 0, 0, 0, 0.1;

      for (int leg = 0; leg < 4; leg++) {
        _legController->commands[leg].kpJoint = kpMat;
        _legController->commands[leg].kdJoint = kdMat;
      }
    } else {
      // Run Control 
      _robot_ctrl->runController();

      // Update Visualization
      _robot_ctrl->updateVisualization();
    }
  }



  // Visualization (will make this into a separate function later)
  for (int leg = 0; leg < 4; leg++) {
    for (int joint = 0; joint < 3; joint++) {
      cheetahMainVisualization->q[leg * 3 + joint] =
        _legController->datas[leg].q[joint];
    }
  }
  cheetahMainVisualization->p.setZero();
  cheetahMainVisualization->p = _stateEstimate.position;

  // Sets the leg controller commands for the robot appropriate commands
  finalizeStep();
}

Here is the call graph for this function:

Here is the caller graph for this function:

void RobotRunner::setupStep ( )

private

Definition at line 123 of file RobotRunner.cpp.

References _cheaterModeEnabled, _legController, CHEETAH_3, controlParameters, initializeStateEstimator(), MINI_CHEETAH, robotType, LegController< T >::setEnabled(), LegController< T >::setMaxTorqueCheetah3(), spiData, tiBoardData, LegController< T >::updateData(), and LegController< T >::zeroCommand().

                            {
  // Update the leg data
  if (robotType == RobotType::MINI_CHEETAH) {
    _legController->updateData(spiData);
  } else if (robotType == RobotType::CHEETAH_3) {
    _legController->updateData(tiBoardData);
  } else {
    assert(false);
  }

  // Setup the leg controller for a new iteration
  _legController->zeroCommand();
  _legController->setEnabled(true);
  _legController->setMaxTorqueCheetah3(208.5);

  // state estimator
  // check transition to cheater mode:
  if (!_cheaterModeEnabled && controlParameters->cheater_mode) {
    printf("[RobotRunner] Transitioning to Cheater Mode...\n");
    initializeStateEstimator(true);
    // todo any configuration
    _cheaterModeEnabled = true;
  }

  // check transition from cheater mode:
  if (_cheaterModeEnabled && !controlParameters->cheater_mode) {
    printf("[RobotRunner] Transitioning from Cheater Mode...\n");
    initializeStateEstimator(false);
    // todo any configuration
    _cheaterModeEnabled = false;
  }

  // todo safety checks, sanity checks, etc...
}

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

bool RobotRunner::_cheaterModeEnabled = false

private

Definition at line 71 of file RobotRunner.h.

DesiredStateCommand<float>* RobotRunner::_desiredStateCommand

private

Definition at line 72 of file RobotRunner.h.

float RobotRunner::_ini_yaw

private

Definition at line 59 of file RobotRunner.h.

u64 RobotRunner::_iterations = 0

private

Definition at line 80 of file RobotRunner.h.

JPosInitializer<float>* RobotRunner::_jpos_initializer

private

Definition at line 66 of file RobotRunner.h.

lcm::LCM RobotRunner::_lcm

private

Definition at line 73 of file RobotRunner.h.

LegController<float>* RobotRunner::_legController = nullptr

private

Definition at line 68 of file RobotRunner.h.

FloatingBaseModel<float> RobotRunner::_model

private

Definition at line 79 of file RobotRunner.h.

Quadruped<float> RobotRunner::_quadruped

private

Definition at line 67 of file RobotRunner.h.

RobotController* RobotRunner::_robot_ctrl

Definition at line 44 of file RobotRunner.h.

StateEstimate<float> RobotRunner::_stateEstimate

private

Definition at line 69 of file RobotRunner.h.

StateEstimatorContainer<float>* RobotRunner::_stateEstimator

private

Definition at line 70 of file RobotRunner.h.

CheaterState<double>* RobotRunner::cheaterState

Definition at line 49 of file RobotRunner.h.

CheetahVisualization* RobotRunner::cheetahMainVisualization

Definition at line 56 of file RobotRunner.h.

RobotControlParameters* RobotRunner::controlParameters

Definition at line 54 of file RobotRunner.h.

GamepadCommand* RobotRunner::driverCommand

Definition at line 46 of file RobotRunner.h.

int RobotRunner::iter = 0

private

Definition at line 61 of file RobotRunner.h.

leg_control_command_lcmt RobotRunner::leg_control_command_lcm

private

Definition at line 74 of file RobotRunner.h.

leg_control_data_lcmt RobotRunner::leg_control_data_lcm

private

Definition at line 76 of file RobotRunner.h.

RobotType RobotRunner::robotType

Definition at line 47 of file RobotRunner.h.

SpiCommand* RobotRunner::spiCommand

Definition at line 51 of file RobotRunner.h.

SpiData* RobotRunner::spiData

Definition at line 50 of file RobotRunner.h.

state_estimator_lcmt RobotRunner::state_estimator_lcm

private

Definition at line 75 of file RobotRunner.h.

TiBoardCommand* RobotRunner::tiBoardCommand

Definition at line 52 of file RobotRunner.h.

TiBoardData* RobotRunner::tiBoardData

Definition at line 53 of file RobotRunner.h.

VectorNavData* RobotRunner::vectorNavData

Definition at line 48 of file RobotRunner.h.

VisualizationData* RobotRunner::visualizationData

Definition at line 55 of file RobotRunner.h.

---

The documentation for this class was generated from the following files:

• RobotRunner.h
• RobotRunner.cpp